diff --git a/darwin_driver/CMakeLists.txt b/darwin_driver/CMakeLists.txt
index 88290149682359dba5c0f8450201052963ad7c7f..aceb2582a2afb2a25239db1a53ef5f3b7c78ee25 100644
--- a/darwin_driver/CMakeLists.txt
+++ b/darwin_driver/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_driver std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver iri_action_server std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -62,7 +62,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_driver std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs
+ CATKIN_DEPENDS iri_base_driver iri_action_server std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
diff --git a/darwin_driver/include/darwin_driver.h b/darwin_driver/include/darwin_driver.h
index 2568811456250988551d0aede1477a42a9bde864..3fbc9217372904d6ce0a3200e07388ac32c87cd0 100644
--- a/darwin_driver/include/darwin_driver.h
+++ b/darwin_driver/include/darwin_driver.h
@@ -53,6 +53,35 @@ typedef struct
   double rot_speed;
 }TWalkParams;
 
+typedef struct
+{
+  double shift_weight_left_time;
+  double rise_right_foot_time;
+  double advance_right_foot_time;
+  double contact_right_foot_time;
+  double shift_weight_right_time;
+  double rise_left_foot_time;
+  double advance_left_foot_time;
+  double contact_left_foot_time;
+  double center_weight_time;
+  double x_offset;
+  double y_offset;
+  double z_offset;
+  double roll_offset;
+  double pitch_offset;
+  double yaw_offset;
+  double y_shift;
+  double x_shift;
+  double z_overshoot;
+  double z_height;
+  double hip_pitch_offset;
+  double roll_shift;
+  double pitch_shift;
+  double yaw_shift;
+  double y_spread;
+  double x_shift_body;
+}TStairsParams;
+
 /**
  * \brief IRI ROS Specific Driver Class
  *
@@ -78,6 +107,7 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
     // private attributes and methods
     CDarwinRobot *darwin; //de darwin_robot_driver
     TWalkParams walk_params;
+    TStairsParams stairs_params;
   public:
    /**
     * \brief define config type
@@ -200,6 +230,14 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
     bool is_walking(void);
     void walk(double step_x_m,double step_y_m,double step_rad);
     double walk_get_timeout(void);
+    /* stairs interface */
+    void stairs_set_params(TStairsParams *params);
+    void stairs_get_params(TStairsParams *params);
+    void stairs_go_up(void);
+    void stairs_go_down(void);
+    void stairs_stop(void);
+    bool stairs_is_active(void);
+    stairs_phase_t stairs_get_phase(void);
     // joint motion interface
     void joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
     void joints_start(joints_grp_t group);
diff --git a/darwin_driver/include/darwin_driver_node.h b/darwin_driver/include/darwin_driver_node.h
index 271fb4319fa82d25c779ca80a9463974d4b27aaf..5aed1741266b131d9b4bb11bf566652a71d18283 100644
--- a/darwin_driver/include/darwin_driver_node.h
+++ b/darwin_driver/include/darwin_driver_node.h
@@ -39,6 +39,8 @@
 #include <std_msgs/Int8.h>
 
 // [service client headers]
+#include <humanoid_common_msgs/get_stairs_params.h>
+#include <humanoid_common_msgs/set_stairs_params.h>
 #include <humanoid_common_msgs/get_smart_charger_config.h>
 #include <humanoid_common_msgs/set_smart_charger_config.h>
 //
@@ -49,6 +51,7 @@
 #include <humanoid_common_msgs/set_servo_modules.h>
 
 // [action server client headers]
+#include <humanoid_common_msgs/humanoid_stairsAction.h>
 #include <control_msgs/JointTrajectoryAction.h>
 #include <humanoid_common_msgs/humanoid_follow_targetAction.h>
 #include <iri_action_server/iri_action_server.h>
@@ -113,6 +116,18 @@ class DarwinDriverNode : public iri_base_driver::IriBaseNodeDriver<DarwinDriver>
     ros::Time walk_timeout;
 
     // [service attributes]
+    ros::ServiceServer get_stairs_params_server_;
+    bool get_stairs_paramsCallback(humanoid_common_msgs::get_stairs_params::Request &req, humanoid_common_msgs::get_stairs_params::Response &res);
+    pthread_mutex_t get_stairs_params_mutex_;
+    void get_stairs_params_mutex_enter(void);
+    void get_stairs_params_mutex_exit(void);
+
+    ros::ServiceServer set_stairs_params_server_;
+    bool set_stairs_paramsCallback(humanoid_common_msgs::set_stairs_params::Request &req, humanoid_common_msgs::set_stairs_params::Response &res);
+    pthread_mutex_t set_stairs_params_mutex_;
+    void set_stairs_params_mutex_enter(void);
+    void set_stairs_params_mutex_exit(void);
+
 // Get smart charger module config
     ros::ServiceServer get_smart_charger_config_server_;
     bool get_smart_charger_configCallback(humanoid_common_msgs::get_smart_charger_config::Request &req, humanoid_common_msgs::get_smart_charger_config::Response &res);
@@ -173,6 +188,17 @@ class DarwinDriverNode : public iri_base_driver::IriBaseNodeDriver<DarwinDriver>
     // [client attributes]
 
     // [action server attributes]
+    IriActionServer<humanoid_common_msgs::humanoid_stairsAction> climb_stairs_aserver_;
+    void climb_stairsStartCallback(const humanoid_common_msgs::humanoid_stairsGoalConstPtr& goal);
+    void climb_stairsStopCallback(void);
+    bool climb_stairsIsFinishedCallback(void);
+    bool climb_stairsHasSucceededCallback(void);
+    void climb_stairsGetResultCallback(humanoid_common_msgs::humanoid_stairsResultPtr& result);
+    void climb_stairsGetFeedbackCallback(humanoid_common_msgs::humanoid_stairsFeedbackPtr& feedback);
+    bool climb_stairs_active;
+    bool climb_stairs_succeeded;
+    bool climb_stairs_finished;
+
     IriActionServer<control_msgs::JointTrajectoryAction> joint_trajectory_aserver_;
     void joint_trajectoryStartCallback(const control_msgs::JointTrajectoryGoalConstPtr& goal);
     void joint_trajectoryStopCallback(void);
diff --git a/darwin_driver/src/darwin_driver.cpp b/darwin_driver/src/darwin_driver.cpp
index ac13f5e00d68c92c79309a0fa4d5607299bccb7a..98c51a2c711991c79b711b00bce73ab0bb0d7ea2 100644
--- a/darwin_driver/src/darwin_driver.cpp
+++ b/darwin_driver/src/darwin_driver.cpp
@@ -35,6 +35,32 @@ bool DarwinDriver::openDriver(void)
     this->walk_params.arm_swing_gain=this->darwin->walk_get_arm_swing_gain();
     this->walk_params.trans_speed=this->darwin->walk_get_trans_speed();
     this->walk_params.rot_speed=this->darwin->walk_get_rot_speed();
+    // get the current stairs parameters
+    this->stairs_params.shift_weight_left_time=this->darwin->stairs_get_phase_time(SHIFT_WEIGHT_LEFT);
+    this->stairs_params.rise_right_foot_time=this->darwin->stairs_get_phase_time(RISE_RIGHT_FOOT);
+    this->stairs_params.advance_right_foot_time=this->darwin->stairs_get_phase_time(ADVANCE_RIGHT_FOOT);
+    this->stairs_params.contact_right_foot_time=this->darwin->stairs_get_phase_time(CONTACT_RIGHT_FOOT);
+    this->stairs_params.shift_weight_right_time=this->darwin->stairs_get_phase_time(SHIFT_WEIGHT_RIGHT);
+    this->stairs_params.rise_left_foot_time=this->darwin->stairs_get_phase_time(RISE_LEFT_FOOT);
+    this->stairs_params.advance_left_foot_time=this->darwin->stairs_get_phase_time(ADVANCE_LEFT_FOOT);
+    this->stairs_params.contact_left_foot_time=this->darwin->stairs_get_phase_time(CONTACT_LEFT_FOOT);
+    this->stairs_params.center_weight_time=this->darwin->stairs_get_phase_time(CENTER_WEIGHT);
+    this->stairs_params.x_offset=this->darwin->stairs_get_x_offset();
+    this->stairs_params.y_offset=this->darwin->stairs_get_y_offset();
+    this->stairs_params.z_offset=this->darwin->stairs_get_z_offset();
+    this->stairs_params.roll_offset=this->darwin->stairs_get_roll_offset();
+    this->stairs_params.pitch_offset=this->darwin->stairs_get_pitch_offset();
+    this->stairs_params.yaw_offset=this->darwin->stairs_get_yaw_offset();
+    this->stairs_params.y_shift=this->darwin->stairs_get_y_shift();
+    this->stairs_params.x_shift=this->darwin->stairs_get_x_shift();
+    this->stairs_params.z_overshoot=this->darwin->stairs_get_z_overshoot();
+    this->stairs_params.z_height=this->darwin->stairs_get_z_height();
+    this->stairs_params.hip_pitch_offset=this->darwin->stairs_get_hip_pitch_offset();
+    this->stairs_params.roll_shift=this->darwin->stairs_get_roll_shift();
+    this->stairs_params.pitch_shift=this->darwin->stairs_get_pitch_shift();
+    this->stairs_params.yaw_shift=this->darwin->stairs_get_yaw_shift();
+    this->stairs_params.y_spread=this->darwin->stairs_get_y_spread();
+    this->stairs_params.x_shift_body=this->darwin->stairs_get_x_shift_body();
  
     return true;
   }catch(CException &e){
@@ -471,6 +497,202 @@ double DarwinDriver::walk_get_timeout(void)
   return this->config_.cmd_vel_timeout;
 }
 
+/* stairs interface */
+void DarwinDriver::stairs_set_params(TStairsParams *params)
+{
+  if(this->darwin!=NULL)
+  {
+    if(this->stairs_params.shift_weight_left_time!=params->shift_weight_left_time)
+    {
+      this->stairs_params.shift_weight_left_time=params->shift_weight_left_time;
+      this->darwin->stairs_set_phase_time(SHIFT_WEIGHT_LEFT,params->shift_weight_left_time);
+    }
+    if(this->stairs_params.rise_right_foot_time!=params->rise_right_foot_time)
+    {
+      this->stairs_params.rise_right_foot_time=params->rise_right_foot_time;
+      this->darwin->stairs_set_phase_time(RISE_RIGHT_FOOT,params->rise_right_foot_time);
+    }
+    if(this->stairs_params.advance_right_foot_time!=params->advance_right_foot_time)
+    {
+      this->stairs_params.advance_right_foot_time=params->advance_right_foot_time;
+      this->darwin->stairs_set_phase_time(ADVANCE_RIGHT_FOOT,params->advance_right_foot_time);
+    }
+    if(this->stairs_params.contact_right_foot_time!=params->contact_right_foot_time)
+    {
+      this->stairs_params.contact_right_foot_time=params->contact_right_foot_time;
+      this->darwin->stairs_set_phase_time(CONTACT_RIGHT_FOOT,params->contact_right_foot_time);
+    }
+    if(this->stairs_params.shift_weight_right_time!=params->shift_weight_right_time)
+    {
+      this->stairs_params.shift_weight_right_time=params->shift_weight_right_time;
+      this->darwin->stairs_set_phase_time(SHIFT_WEIGHT_RIGHT,params->shift_weight_right_time);
+    }
+    if(this->stairs_params.rise_left_foot_time!=params->rise_left_foot_time)
+    {
+      this->stairs_params.rise_left_foot_time=params->rise_left_foot_time;
+      this->darwin->stairs_set_phase_time(RISE_LEFT_FOOT,params->rise_left_foot_time);
+    }
+    if(this->stairs_params.advance_left_foot_time!=params->advance_left_foot_time)
+    {
+      this->stairs_params.advance_left_foot_time=params->advance_left_foot_time;
+      this->darwin->stairs_set_phase_time(ADVANCE_LEFT_FOOT,params->advance_left_foot_time);
+    }
+    if(this->stairs_params.contact_left_foot_time!=params->contact_left_foot_time)
+    {
+      this->stairs_params.contact_left_foot_time=params->contact_left_foot_time;
+      this->darwin->stairs_set_phase_time(CONTACT_LEFT_FOOT,params->contact_left_foot_time);
+    }
+    if(this->stairs_params.center_weight_time!=params->center_weight_time)
+    {
+      this->stairs_params.center_weight_time=params->center_weight_time;
+      this->darwin->stairs_set_phase_time(CENTER_WEIGHT,params->center_weight_time);
+    }
+    if(this->stairs_params.x_offset!=params->x_offset)
+    {
+      this->stairs_params.x_offset=params->x_offset;
+      this->darwin->stairs_set_x_offset(params->x_offset);
+    }
+    if(this->stairs_params.y_offset!=params->y_offset)
+    {
+      this->stairs_params.y_offset=params->y_offset;
+      this->darwin->stairs_set_y_offset(params->y_offset);
+    }
+    if(this->stairs_params.z_offset!=params->z_offset)
+    {
+      this->stairs_params.z_offset=params->z_offset;
+      this->darwin->stairs_set_z_offset(params->z_offset);
+    }
+    if(this->stairs_params.roll_offset!=params->roll_offset)
+    {
+      this->stairs_params.roll_offset=params->roll_offset;
+      this->darwin->stairs_set_roll_offset(params->roll_offset);
+    }
+    if(this->stairs_params.pitch_offset!=params->pitch_offset)
+    {
+      this->stairs_params.pitch_offset=params->pitch_offset;
+      this->darwin->stairs_set_pitch_offset(params->pitch_offset);
+    }
+    if(this->stairs_params.yaw_offset!=params->yaw_offset)
+    {
+      this->stairs_params.yaw_offset=params->yaw_offset;
+      this->darwin->stairs_set_yaw_offset(params->yaw_offset);
+    }
+    if(this->stairs_params.y_shift!=params->y_shift)
+    {
+      this->stairs_params.y_shift=params->y_shift;
+      this->darwin->stairs_set_y_shift(params->y_shift);
+    }
+    if(this->stairs_params.x_shift!=params->x_shift)
+    {
+      this->stairs_params.x_shift=params->x_shift;
+      this->darwin->stairs_set_x_shift(params->x_shift);
+    }
+    if(this->stairs_params.z_overshoot!=params->z_overshoot)
+    {
+      this->stairs_params.z_overshoot=params->z_overshoot;
+      this->darwin->stairs_set_z_overshoot(params->z_overshoot);
+    }
+    if(this->stairs_params.z_height!=params->z_height)
+    {
+      this->stairs_params.yaw_offset=params->z_height;
+      this->darwin->stairs_set_z_height(params->z_height);
+    }
+    if(this->stairs_params.hip_pitch_offset!=params->hip_pitch_offset)
+    {
+      this->stairs_params.hip_pitch_offset=params->hip_pitch_offset;
+      this->darwin->stairs_set_hip_pitch_offset(params->hip_pitch_offset);
+    }
+    if(this->stairs_params.roll_shift!=params->roll_shift)
+    {
+      this->stairs_params.roll_shift=params->roll_shift;
+      this->darwin->stairs_set_roll_shift(params->roll_shift);
+    }
+    if(this->stairs_params.pitch_shift!=params->pitch_shift)
+    {
+      this->stairs_params.pitch_shift=params->pitch_shift;
+      this->darwin->stairs_set_pitch_shift(params->pitch_shift);
+    }
+    if(this->stairs_params.yaw_shift!=params->yaw_shift)
+    {
+      this->stairs_params.yaw_shift=params->yaw_shift;
+      this->darwin->stairs_set_yaw_shift(params->yaw_shift);
+    }
+    if(this->stairs_params.y_spread!=params->y_spread)
+    {
+      this->stairs_params.y_spread=params->y_spread;
+      this->darwin->stairs_set_y_spread(params->y_spread);
+    }
+    if(this->stairs_params.x_shift_body!=params->x_shift_body)
+    {
+      this->stairs_params.x_shift_body=params->x_shift_body;
+      this->darwin->stairs_set_x_shift_body(params->x_shift_body);
+    }
+  }
+}
+
+void DarwinDriver::stairs_get_params(TStairsParams *params)
+{
+  params->shift_weight_left_time=this->stairs_params.shift_weight_left_time;
+  params->rise_right_foot_time=this->stairs_params.rise_right_foot_time;
+  params->advance_right_foot_time=this->stairs_params.advance_right_foot_time;
+  params->contact_right_foot_time=this->stairs_params.contact_right_foot_time;
+  params->shift_weight_right_time=this->stairs_params.shift_weight_right_time;
+  params->rise_left_foot_time=this->stairs_params.rise_left_foot_time;
+  params->advance_left_foot_time=this->stairs_params.advance_left_foot_time;
+  params->contact_left_foot_time=this->stairs_params.contact_left_foot_time;
+  params->center_weight_time=this->stairs_params.center_weight_time;
+  params->x_offset=this->stairs_params.x_offset;
+  params->y_offset=this->stairs_params.y_offset;
+  params->z_offset=this->stairs_params.z_offset;
+  params->roll_offset=this->stairs_params.roll_offset;
+  params->pitch_offset=this->stairs_params.pitch_offset;
+  params->yaw_offset=this->stairs_params.yaw_offset;
+  params->y_shift=this->stairs_params.y_shift;
+  params->x_shift=this->stairs_params.x_shift;
+  params->z_overshoot=this->stairs_params.z_overshoot;
+  params->z_height=this->stairs_params.z_height;
+  params->hip_pitch_offset=this->stairs_params.hip_pitch_offset;
+  params->roll_shift=this->stairs_params.roll_shift;
+  params->pitch_shift=this->stairs_params.pitch_shift;
+  params->yaw_shift=this->stairs_params.yaw_shift;
+  params->y_spread=this->stairs_params.y_spread;
+  params->x_shift_body=this->stairs_params.x_shift_body;
+}
+
+void DarwinDriver::stairs_go_up(void)
+{
+  if(this->darwin!=NULL)
+    this->darwin->stairs_go_up();
+}
+
+void DarwinDriver::stairs_go_down(void)
+{
+  if(this->darwin!=NULL)
+    this->darwin->stairs_go_down();
+}
+
+void DarwinDriver::stairs_stop(void)
+{
+  if(this->darwin!=NULL)
+    this->darwin->stairs_stop();
+}
+
+bool DarwinDriver::stairs_is_active(void)
+{
+  if(this->darwin!=NULL)
+    return this->darwin->stairs_is_active();
+  else
+    return false;
+}
+
+stairs_phase_t DarwinDriver::stairs_get_phase(void)
+{
+  if(this->darwin!=NULL)
+    return this->darwin->stairs_get_phase();
+  else
+    return (stairs_phase_t)0x00;
+}
+
 // joint motion interface
 void DarwinDriver::joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
 {
diff --git a/darwin_driver/src/darwin_driver_node.cpp b/darwin_driver/src/darwin_driver_node.cpp
index c1090e719baa1a1a8dc2c2f7caf0792088ee3435..f899e96d70fdc289f817d454a2931fced0e2e1ef 100644
--- a/darwin_driver/src/darwin_driver_node.cpp
+++ b/darwin_driver/src/darwin_driver_node.cpp
@@ -34,6 +34,7 @@ const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
 
 DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) : 
   iri_base_driver::IriBaseNodeDriver<DarwinDriver>(nh),
+  climb_stairs_aserver_(public_node_handle_, "climb_stairs"),
   joint_trajectory_aserver_(public_node_handle_, "robot/joint_trajectory"),
   head_follow_target_aserver_(public_node_handle_, "robot/head_follow_target"),
   motion_action_aserver_(public_node_handle_, "robot/motion_action")
@@ -60,6 +61,12 @@ DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) :
 
   
   // [init services]
+  this->get_stairs_params_server_ = this->public_node_handle_.advertiseService("get_stairs_params", &DarwinDriverNode::get_stairs_paramsCallback, this);
+  pthread_mutex_init(&this->get_stairs_params_mutex_,NULL);
+
+  this->set_stairs_params_server_ = this->public_node_handle_.advertiseService("set_stairs_params", &DarwinDriverNode::set_stairs_paramsCallback, this);
+  pthread_mutex_init(&this->set_stairs_params_mutex_,NULL);
+
 // Get smart charger module configuration
   this->get_smart_charger_config_server_ = this->public_node_handle_.advertiseService("get_smart_charger_config", &DarwinDriverNode::get_smart_charger_configCallback, this);
   pthread_mutex_init(&this->get_smart_charger_config_mutex_,NULL);
@@ -92,6 +99,17 @@ DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) :
   // [init clients]
   
   // [init action servers]
+  climb_stairs_aserver_.registerStartCallback(boost::bind(&DarwinDriverNode::climb_stairsStartCallback, this, _1));
+  climb_stairs_aserver_.registerStopCallback(boost::bind(&DarwinDriverNode::climb_stairsStopCallback, this));
+  climb_stairs_aserver_.registerIsFinishedCallback(boost::bind(&DarwinDriverNode::climb_stairsIsFinishedCallback, this));
+  climb_stairs_aserver_.registerHasSucceedCallback(boost::bind(&DarwinDriverNode::climb_stairsHasSucceededCallback, this));
+  climb_stairs_aserver_.registerGetResultCallback(boost::bind(&DarwinDriverNode::climb_stairsGetResultCallback, this, _1));
+  climb_stairs_aserver_.registerGetFeedbackCallback(boost::bind(&DarwinDriverNode::climb_stairsGetFeedbackCallback, this, _1));
+  climb_stairs_aserver_.start();
+  this->climb_stairs_active=false;
+  this->climb_stairs_succeeded=false;
+  this->climb_stairs_finished=false;
+
   joint_trajectory_aserver_.registerStartCallback(boost::bind(&DarwinDriverNode::joint_trajectoryStartCallback, this, _1));
   joint_trajectory_aserver_.registerStopCallback(boost::bind(&DarwinDriverNode::joint_trajectoryStopCallback, this));
   joint_trajectory_aserver_.registerIsFinishedCallback(boost::bind(&DarwinDriverNode::joint_trajectoryIsFinishedCallback, this));
@@ -141,33 +159,16 @@ void DarwinDriverNode::mainNodeThread(void)
   // [fill msg Header if necessary]
 
   // [fill msg structures]
-  // Initialize the topic message structure
-  //this->smart_charger_data_smart_charger_data_msg_.data = my_var;
-
 
   // Initialize the topic message structure
-  //this->raw_imu_Imu_msg_.data = my_var;
 
   // [fill srv structure and make request to the server]
-  
-  // To finish the action server with success
-  //this->head_follow_target_succeeded=true;
-  //this->head_follow_target_finished=true;
-  // To finish the action server with failure
-  //this->head_follow_target_succeeded=false;
-  //this->head_follow_target_finished=true;
-
-  // IMPORTANT: it is better to use the boolean variables to control the
-  // behavior of the action server instead of direclty calling the action server
-  // class functions.
+ 
+  // [fill action structure and make request to the action server]
 
   // [publish messages]
 
   // Uncomment the following line to publish the topic message
-  //this->raw_imu_publisher_.publish(this->raw_imu_Imu_msg_);
-
-  // Initialize the topic message structure
-  // initialize the joint states message
   try{
     this->joint_states_JointState_msg_.name.resize(MAX_NUM_SERVOS);
     this->joint_states_JointState_msg_.position.resize(MAX_NUM_SERVOS);
@@ -359,6 +360,135 @@ void DarwinDriverNode::cmd_vel_mutex_exit(void)
 
 
 /*  [service callbacks] */
+bool DarwinDriverNode::get_stairs_paramsCallback(humanoid_common_msgs::get_stairs_params::Request &req, humanoid_common_msgs::get_stairs_params::Response &res)
+{
+  TStairsParams params;
+
+  ROS_INFO("DarwinDriverNode::get_stairs_paramsCallback: New Request Received!");
+
+  //use appropiate mutex to shared variables if necessary
+  try{
+    this->driver_.lock();
+    this->get_stairs_params_mutex_enter();
+    this->driver_.stairs_get_params(&params);
+    res.params.PHASE1_TIME=params.shift_weight_left_time;
+    res.params.PHASE2_TIME=params.rise_right_foot_time;
+    res.params.PHASE3_TIME=params.advance_right_foot_time;
+    res.params.PHASE4_TIME=params.contact_right_foot_time;
+    res.params.PHASE5_TIME=params.shift_weight_right_time;
+    res.params.PHASE6_TIME=params.rise_left_foot_time;
+    res.params.PHASE7_TIME=params.advance_left_foot_time;
+    res.params.PHASE8_TIME=params.contact_left_foot_time;
+    res.params.PHASE9_TIME=params.center_weight_time;
+    res.params.X_OFFSET=params.x_offset;
+    res.params.Y_OFFSET=params.y_offset;
+    res.params.Z_OFFSET=params.z_offset;
+    res.params.R_OFFSET=params.roll_offset;
+    res.params.P_OFFSET=params.pitch_offset;
+    res.params.A_OFFSET=params.yaw_offset;
+    res.params.Y_SHIFT=params.y_shift;
+    res.params.X_SHIFT=params.x_shift;
+    res.params.Z_OVERSHOOT=params.z_overshoot;
+    res.params.Z_HEIGHT=params.z_height;
+    res.params.HIP_PITCH_OFFSET=params.hip_pitch_offset;
+    res.params.R_SHIFT=params.roll_shift;
+    res.params.P_SHIFT=params.pitch_shift;
+    res.params.A_SHIFT=params.yaw_shift;
+    res.params.Y_SPREAD=params.y_spread;
+    res.params.X_SHIFT_BODY=params.x_shift_body;
+
+    this->get_stairs_params_mutex_exit();
+    this->driver_.unlock();
+  }catch(CException &e){
+    ROS_WARN_STREAM(e.what());
+    this->get_stairs_params_mutex_exit();
+    this->driver_.unlock();
+  }
+  return true;
+}
+
+void DarwinDriverNode::get_stairs_params_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->get_stairs_params_mutex_);
+}
+
+void DarwinDriverNode::get_stairs_params_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->get_stairs_params_mutex_);
+}
+
+bool DarwinDriverNode::set_stairs_paramsCallback(humanoid_common_msgs::set_stairs_params::Request &req, humanoid_common_msgs::set_stairs_params::Response &res)
+{
+  TStairsParams params;
+
+  ROS_INFO("DarwinDriverNode::set_stairs_paramsCallback: New Request Received!");
+
+  try{
+    this->driver_.lock();
+    this->get_stairs_params_mutex_enter();
+    params.shift_weight_left_time=req.params.PHASE1_TIME;
+    params.rise_right_foot_time=req.params.PHASE2_TIME;
+    params.advance_right_foot_time=req.params.PHASE3_TIME;
+    params.contact_right_foot_time=req.params.PHASE4_TIME;
+    params.shift_weight_right_time=req.params.PHASE5_TIME;
+    params.rise_left_foot_time=req.params.PHASE6_TIME;
+    params.advance_left_foot_time=req.params.PHASE7_TIME;
+    params.contact_left_foot_time=req.params.PHASE8_TIME;
+    params.center_weight_time=req.params.PHASE9_TIME;
+    params.x_offset=req.params.X_OFFSET;
+    params.y_offset=req.params.Y_OFFSET;
+    params.z_offset=req.params.Z_OFFSET;
+    params.roll_offset=req.params.R_OFFSET;
+    params.pitch_offset=req.params.P_OFFSET;
+    params.yaw_offset=req.params.A_OFFSET;
+    params.y_shift=req.params.Y_SHIFT;
+    params.x_shift=req.params.X_SHIFT;
+    params.z_overshoot=req.params.Z_OVERSHOOT;
+    params.z_height=req.params.Z_HEIGHT;
+    params.hip_pitch_offset=req.params.HIP_PITCH_OFFSET;
+    params.roll_shift=req.params.R_SHIFT;
+    params.pitch_shift=req.params.P_SHIFT;
+    params.yaw_shift=req.params.A_SHIFT;
+    params.y_spread=req.params.Y_SPREAD;
+    params.x_shift_body=req.params.X_SHIFT_BODY;
+    this->driver_.stairs_set_params(&params);
+    res.ret=true;
+    this->get_stairs_params_mutex_exit();
+    this->driver_.unlock();
+  }catch(CException &e){
+    ROS_WARN_STREAM(e.what());
+    res.ret=false;
+    this->get_stairs_params_mutex_exit();
+    this->driver_.unlock();
+  }
+  
+  return true;
+
+  //use appropiate mutex to shared variables if necessary
+  //this->driver_.lock();
+  //this->set_stairs_params_mutex_enter();
+
+  //ROS_INFO("DarwinDriverNode::set_stairs_paramsCallback: Processing New Request!");
+  //do operations with req and output on res
+  //res.data2 = req.data1 + my_var;
+
+  //unlock previously blocked shared variables
+  //this->set_stairs_params_mutex_exit();
+  //this->driver_.unlock();
+
+  return true;
+}
+
+void DarwinDriverNode::set_stairs_params_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->set_stairs_params_mutex_);
+}
+
+void DarwinDriverNode::set_stairs_params_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->set_stairs_params_mutex_);
+}
+
 bool DarwinDriverNode::get_smart_charger_configCallback(humanoid_common_msgs::get_smart_charger_config::Request &req, humanoid_common_msgs::get_smart_charger_config::Response &res)
 {
   ROS_INFO("DarwinDriverNode::get_smart_charger_configCallback: New Request Received!");
@@ -732,6 +862,105 @@ void DarwinDriverNode::set_servo_modules_mutex_exit(void)
 
 
 /*  [action callbacks] */
+void DarwinDriverNode::climb_stairsStartCallback(const humanoid_common_msgs::humanoid_stairsGoalConstPtr& goal)
+{
+  try{
+    this->driver_.lock();
+    //check goal
+    if(goal->up)
+      this->driver_.stairs_go_up();
+    else
+      this->driver_.stairs_go_down();
+    this->climb_stairs_active=true;
+    this->climb_stairs_succeeded=false;
+    this->climb_stairs_finished=false;
+    //execute goal
+    this->driver_.unlock();
+  }catch(CException &e){
+    ROS_WARN_STREAM(e.what());
+    this->climb_stairs_active=false;
+    this->climb_stairs_succeeded=false;
+    this->climb_stairs_finished=true;
+    this->driver_.unlock();
+  }
+}
+
+void DarwinDriverNode::climb_stairsStopCallback(void)
+{
+  try{
+    this->driver_.lock();
+    //stop action
+    this->driver_.stairs_stop();
+    this->climb_stairs_active=false;
+    this->driver_.unlock();
+  }catch(CException &e){
+    ROS_WARN_STREAM(e.what());
+    this->driver_.unlock();
+  }
+}
+
+bool DarwinDriverNode::climb_stairsIsFinishedCallback(void)
+{
+  bool ret = false;
+
+  try{
+    this->driver_.lock();
+    //if action has finish for any reason
+    if(this->driver_.stairs_is_active())
+      this->climb_stairs_finished=false;
+    else
+    {
+      this->climb_stairs_finished=true;
+      this->climb_stairs_succeeded=true;
+    }
+    ret = this->climb_stairs_finished;
+    this->driver_.unlock();
+    return ret;
+  }catch(CException &e){
+    ROS_WARN_STREAM(e.what());
+    this->driver_.unlock();
+  }
+}
+
+bool DarwinDriverNode::climb_stairsHasSucceededCallback(void)
+{
+  bool ret = false;
+
+  try{
+    this->driver_.lock();
+    //if goal was accomplished
+    ret = this->climb_stairs_succeeded;
+    this->climb_stairs_active=false;
+    this->driver_.unlock();
+    return ret;
+  }catch(CException &e){
+    ROS_WARN_STREAM(e.what());
+    this->driver_.unlock();
+  }
+}
+
+void DarwinDriverNode::climb_stairsGetResultCallback(humanoid_common_msgs::humanoid_stairsResultPtr& result)
+{
+  this->driver_.lock();
+  //update result data to be sent to client
+  //result->data = data;
+  this->driver_.unlock();
+}
+
+void DarwinDriverNode::climb_stairsGetFeedbackCallback(humanoid_common_msgs::humanoid_stairsFeedbackPtr& feedback)
+{
+  try{
+    this->driver_.lock();
+    //update feedback data to be sent to client
+    feedback->current_phase=this->driver_.stairs_get_phase();
+    //ROS_INFO("feedback: %s", feedback->data.c_str());
+    this->driver_.unlock();
+  }catch(CException &e){
+    ROS_WARN_STREAM(e.what());
+    this->driver_.unlock();
+  }
+}
+
 void DarwinDriverNode::joint_trajectoryStartCallback(const control_msgs::JointTrajectoryGoalConstPtr& goal)
 {
   unsigned int i=0,j=0;
@@ -1086,6 +1315,8 @@ void DarwinDriverNode::reconfigureNodeHook(int level)
 DarwinDriverNode::~DarwinDriverNode(void)
 {
   // [free dynamic memory]
+  pthread_mutex_destroy(&this->get_stairs_params_mutex_);
+  pthread_mutex_destroy(&this->set_stairs_params_mutex_);
   pthread_mutex_destroy(&this->get_smart_charger_config_mutex_);
   pthread_mutex_destroy(&this->set_smart_charger_config_mutex_);
   pthread_mutex_destroy(&this->leds_mutex_);