diff --git a/ir_feet_sensor/CMakeLists.txt b/ir_feet_sensor/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..19c6fd189fed84df6790023e0b04e5b211cdba43
--- /dev/null
+++ b/ir_feet_sensor/CMakeLists.txt
@@ -0,0 +1,113 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ir_feet_sensor)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ******************************************************************** 
+#                 Add catkin additional components here
+# ******************************************************************** 
+find_package(catkin REQUIRED COMPONENTS iri_base_driver)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ******************************************************************** 
+#           Add system and labrobotica dependencies here
+# ******************************************************************** 
+# find_package(<dependency> REQUIRED)
+find_package(iriutils REQUIRED)
+find_package(comm REQUIRED)
+find_package(iriutils REQUIRED)
+find_package(ir_feet REQUIRED)
+
+# ******************************************************************** 
+#           Add topic, service and action definition here
+# ******************************************************************** 
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages, services and actions with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+# ******************************************************************** 
+#                 Add the dynamic reconfigure file 
+# ******************************************************************** 
+generate_dynamic_reconfigure_options(cfg/IrFeetSensor.cfg)
+
+# ******************************************************************** 
+#                 Add run time dependencies here
+# ******************************************************************** 
+catkin_package(
+#  INCLUDE_DIRS 
+#  LIBRARIES 
+# ******************************************************************** 
+#            Add ROS and IRI ROS run time dependencies
+# ******************************************************************** 
+ CATKIN_DEPENDS iri_base_driver
+# ******************************************************************** 
+#      Add system and labrobotica run time dependencies here
+# ******************************************************************** 
+#  DEPENDS 
+)
+
+###########
+## Build ##
+###########
+
+# ******************************************************************** 
+#                   Add the include directories 
+# ******************************************************************** 
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${comm_INCLUDE_DIR})
+include_directories(${dynamxiel_INCLUDE_DIR})
+include_directories(${ir_feet_INCLUDE_DIR})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/ir_feet_sensor_driver.cpp src/ir_feet_sensor_driver_node.cpp)
+
+# ******************************************************************** 
+#                   Add the libraries
+# ******************************************************************** 
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${comm_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${dynamixel_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${ir_feet_LIBRARY})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ******************************************************************** 
+#               Add message headers dependencies 
+# ******************************************************************** 
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+# ******************************************************************** 
+#               Add dynamic reconfigure dependencies 
+# ******************************************************************** 
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/ir_feet_sensor/cfg/IrFeetSensor.cfg b/ir_feet_sensor/cfg/IrFeetSensor.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..eaa62d96ed21e0ed3ba2c48b613d7841ca89f64f
--- /dev/null
+++ b/ir_feet_sensor/cfg/IrFeetSensor.cfg
@@ -0,0 +1,44 @@
+#! /usr/bin/env python
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of the Willow Garage nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author: 
+
+PACKAGE='ir_feet_sensor'
+
+from driver_base.msg import SensorLevels
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+#       Name                       Type       Reconfiguration level            Description                       Default   Min   Max
+#gen.add("velocity_scale_factor",  double_t,  SensorLevels.RECONFIGURE_STOP,   "Maximum velocity scale factor",  0.5,      0.0,  1.0)
+
+exit(gen.generate(PACKAGE, "IrFeetSensorDriver", "IrFeetSensor"))
diff --git a/ir_feet_sensor/include/.ir_feet_sensor_driver.h.swp b/ir_feet_sensor/include/.ir_feet_sensor_driver.h.swp
new file mode 100644
index 0000000000000000000000000000000000000000..4237eb4bf94311dfa474fc7b8fb78bebb48adf62
Binary files /dev/null and b/ir_feet_sensor/include/.ir_feet_sensor_driver.h.swp differ
diff --git a/ir_feet_sensor/include/ir_feet_sensor_driver.h b/ir_feet_sensor/include/ir_feet_sensor_driver.h
new file mode 100644
index 0000000000000000000000000000000000000000..55103532f129c8e77955673ab9f9f20cbb931e9a
--- /dev/null
+++ b/ir_feet_sensor/include/ir_feet_sensor_driver.h
@@ -0,0 +1,159 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _ir_feet_sensor_driver_h_
+#define _ir_feet_sensor_driver_h_
+
+#include <iri_base_driver/iri_base_driver.h>
+#include <ir_feet_sensor/IrFeetSensorConfig.h>
+
+//include ir_feet_sensor_driver main library
+#include "dynamixelserver_serial.h"
+#include "ir_feet.h"
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ * This class inherits from the IRI Base class IriBaseDriver, which provides the
+ * guidelines to implement any specific driver. The IriBaseDriver class offers an 
+ * easy framework to integrate functional drivers implemented in C++ with the
+ * ROS driver structure. ROS driver_base state transitions are already managed
+ * by IriBaseDriver.
+ *
+ * The IrFeetSensorDriver class must implement all specific driver requirements to
+ * safetely open, close, run and stop the driver at any time. It also must 
+ * guarantee an accessible interface for all driver's parameters.
+ *
+ * The IrFeetSensorConfig.cfg needs to be filled up with those parameters suitable
+ * to be changed dynamically by the ROS dyanmic reconfigure application. The 
+ * implementation of the CIriNode class will manage those parameters through
+ * methods like postNodeOpenHook() and reconfigureNodeHook().
+ *
+ */
+class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
+{
+  private:
+    // private attributes and methods
+    CIRFeet<CDynamixelServerSerial> device;
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the IrFeetSensorConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef ir_feet_sensor::IrFeetSensorConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    IrFeetSensorDriver(void);
+
+   /**
+    * \brief open driver
+    *
+    * In this function, the driver must be openned. Openning errors must be
+    * taken into account. This function is automatically called by 
+    * IriBaseDriver::doOpen(), an state transition is performed if return value
+    * equals true.
+    *
+    * \return bool successful
+    */
+    bool openDriver(void);
+
+   /**
+    * \brief close driver
+    *
+    * In this function, the driver must be closed. Variables related to the
+    * driver state must also be taken into account. This function is automatically
+    * called by IriBaseDriver::doClose(), an state transition is performed if 
+    * return value equals true.
+    *
+    * \return bool successful
+    */
+    bool closeDriver(void);
+
+   /**
+    * \brief start driver
+    *
+    * After this function, the driver and its thread will be started. The driver
+    * and related variables should be properly setup. This function is 
+    * automatically called by IriBaseDriver::doStart(), an state transition is  
+    * performed if return value equals true.
+    *
+    * \return bool successful
+    */
+    bool startDriver(void);
+
+   /**
+    * \brief stop driver
+    *
+    * After this function, the driver's thread will stop its execution. The driver
+    * and related variables should be properly setup. This function is 
+    * automatically called by IriBaseDriver::doStop(), an state transition is  
+    * performed if return value equals true.
+    *
+    * \return bool successful
+    */
+    bool stopDriver(void);
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the 
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& new_cfg, uint32_t level=0);
+
+    // here define all ir_feet_sensor_driver interface methods to retrieve and set
+    // the driver parameters
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~IrFeetSensorDriver(void);
+};
+
+#endif
diff --git a/ir_feet_sensor/include/ir_feet_sensor_driver_node.h b/ir_feet_sensor/include/ir_feet_sensor_driver_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..882899dcec04e5336d4f0c5e3854e35661b92626
--- /dev/null
+++ b/ir_feet_sensor/include/ir_feet_sensor_driver_node.h
@@ -0,0 +1,181 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author 
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+// 
+// IMPORTANT NOTE: This code has been generated through a script from the 
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _ir_feet_sensor_driver_node_h_
+#define _ir_feet_sensor_driver_node_h_
+
+#include <iri_base_driver/iri_base_driver_node.h>
+#include "ir_feet_sensor_driver.h"
+
+// [publisher subscriber headers]
+
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ * This class inherits from the IRI Core class IriBaseNodeDriver<IriBaseDriver>, 
+ * to provide an execution thread to the driver object. A complete framework  
+ * with utilites to test the node functionallity or to add diagnostics to  
+ * specific situations is also given. The inherit template design form allows  
+ * complete access to any IriBaseDriver object implementation.
+ *
+ * As mentioned, tests in the different driver states can be performed through 
+ * class methods such as addNodeOpenedTests() or addNodeRunningTests(). Tests
+ * common to all nodes may be also executed in the pattern class IriBaseNodeDriver.
+ * Similarly to the tests, diagnostics can easyly be added. See ROS Wiki for
+ * more details:
+ * http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic Analyzer)
+ * http://www.ros.org/wiki/self_test/ (Example: Self Test)
+ */
+class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetSensorDriver>
+{
+  private:
+    // [publisher attributes]
+
+    // [subscriber attributes]
+
+    // [service attributes]
+
+    // [client attributes]
+
+    // [action server attributes]
+
+    // [action client attributes]
+
+   /**
+    * \brief post open hook
+    * 
+    * This function is called by IriBaseNodeDriver::postOpenHook(). In this function
+    * specific parameters from the driver must be added so the ROS dynamic 
+    * reconfigure application can update them.
+    */
+    void postNodeOpenHook(void);
+
+  public:
+   /**
+    * \brief constructor
+    *
+    * This constructor mainly creates and initializes the IrFeetSensorDriverNode topics
+    * through the given public_node_handle object. IriBaseNodeDriver attributes 
+    * may be also modified to suit node specifications.
+    *
+    * All kind of ROS topics (publishers, subscribers, servers or clients) can 
+    * be easyly generated with the scripts in the iri_ros_scripts package. Refer
+    * to ROS and IRI Wiki pages for more details:
+    *
+    * http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++)
+    * http://www.ros.org/wiki/ROS/Tutorials/WritingServiceClient(c++)
+    * http://wikiri.upc.es/index.php/Robotics_Lab
+    *
+    * \param nh a reference to the node handle object to manage all ROS topics.
+    */
+    IrFeetSensorDriverNode(ros::NodeHandle& nh);
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~IrFeetSensorDriverNode(void);
+
+  protected:
+   /**
+    * \brief main node thread
+    *
+    * This is the main thread node function. Code written here will be executed
+    * in every node loop while the driver is on running state. Loop frequency 
+    * can be tuned by modifying loop_rate attribute.
+    *
+    * Here data related to the process loop or to ROS topics (mainly data structs
+    * related to the MSG and SRV files) must be updated. ROS publisher objects 
+    * must publish their data in this process. ROS client servers may also
+    * request data to the corresponding server topics.
+    */
+    void mainNodeThread(void);
+
+    // [diagnostic functions]
+
+   /**
+    * \brief node add diagnostics
+    *
+    * In this function ROS diagnostics applied to this specific node may be
+    * added. Common use diagnostics for all nodes are already called from 
+    * IriBaseNodeDriver::addDiagnostics(), which also calls this function. Information
+    * of how ROS diagnostics work can be readen here:
+    * http://www.ros.org/wiki/diagnostics/
+    * http://www.ros.org/doc/api/diagnostic_updater/html/example_8cpp-source.html
+    */
+    void addNodeDiagnostics(void);
+
+    // [driver test functions]
+
+   /**
+    * \brief open status driver tests
+    *
+    * In this function tests checking driver's functionallity when driver_base 
+    * status=open can be added. Common use tests for all nodes are already called
+    * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
+    * please refer to the Self Test example in:
+    * http://www.ros.org/wiki/self_test/
+    */
+    void addNodeOpenedTests(void);
+
+   /**
+    * \brief stop status driver tests
+    *
+    * In this function tests checking driver's functionallity when driver_base 
+    * status=stop can be added. Common use tests for all nodes are already called
+    * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
+    * please refer to the Self Test example in:
+    * http://www.ros.org/wiki/self_test/
+    */
+    void addNodeStoppedTests(void);
+
+   /**
+    * \brief run status driver tests
+    *
+    * In this function tests checking driver's functionallity when driver_base 
+    * status=run can be added. Common use tests for all nodes are already called
+    * from IriBaseNodeDriver tests methods. For more details on how ROS tests work,
+    * please refer to the Self Test example in:
+    * http://www.ros.org/wiki/self_test/
+    */
+    void addNodeRunningTests(void);
+
+   /**
+    * \brief specific node dynamic reconfigure
+    *
+    * This function is called reconfigureHook()
+    * 
+    * \param level integer
+    */
+    void reconfigureNodeHook(int level);
+
+};
+
+#endif
diff --git a/ir_feet_sensor/package.xml b/ir_feet_sensor/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..da2d8914fdd7a7093752844e07256554d581cf1b
--- /dev/null
+++ b/ir_feet_sensor/package.xml
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<package>
+  <name>ir_feet_sensor</name>
+  <version>0.0.0</version>
+  <description>The ir_feet_sensor package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>LGPL</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ir_feet_sensor</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</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>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>iri_base_driver</build_depend>
+  <run_depend>iri_base_driver</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/ir_feet_sensor/src/ir_feet_sensor_driver.cpp b/ir_feet_sensor/src/ir_feet_sensor_driver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6eb4685e63d8c0cd6d29db9ae9d930753038e610
--- /dev/null
+++ b/ir_feet_sensor/src/ir_feet_sensor_driver.cpp
@@ -0,0 +1,56 @@
+#include "ir_feet_sensor_driver.h"
+
+IrFeetSensorDriver::IrFeetSensorDriver(void)
+{
+  //setDriverId(driver string id);
+}
+
+bool IrFeetSensorDriver::openDriver(void)
+{
+  //setDriverId(driver string id);
+
+  return true;
+}
+
+bool IrFeetSensorDriver::closeDriver(void)
+{
+  return true;
+}
+
+bool IrFeetSensorDriver::startDriver(void)
+{
+  return true;
+}
+
+bool IrFeetSensorDriver::stopDriver(void)
+{
+  return true;
+}
+
+void IrFeetSensorDriver::config_update(Config& new_cfg, uint32_t level)
+{
+  this->lock();
+  
+  // depending on current state
+  // update driver with new_cfg data
+  switch(this->getState())
+  {
+    case IrFeetSensorDriver::CLOSED:
+      break;
+
+    case IrFeetSensorDriver::OPENED:
+      break;
+
+    case IrFeetSensorDriver::RUNNING:
+      break;
+  }
+
+  // save the current configuration
+  this->config_=new_cfg;
+
+  this->unlock();
+}
+
+IrFeetSensorDriver::~IrFeetSensorDriver(void)
+{
+}
diff --git a/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp b/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0faf9d0494587993ccebca032dcf03efd6c1b639
--- /dev/null
+++ b/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp
@@ -0,0 +1,82 @@
+#include "ir_feet_sensor_driver_node.h"
+
+IrFeetSensorDriverNode::IrFeetSensorDriverNode(ros::NodeHandle &nh) : 
+  iri_base_driver::IriBaseNodeDriver<IrFeetSensorDriver>(nh)
+{
+  //init class attributes if necessary
+  //this->loop_rate_ = 2;//in [Hz]
+
+  // [init publishers]
+  
+  // [init subscribers]
+  
+  // [init services]
+  
+  // [init clients]
+  
+  // [init action servers]
+  
+  // [init action clients]
+}
+
+void IrFeetSensorDriverNode::mainNodeThread(void)
+{
+  //lock access to driver if necessary
+  this->driver_.lock();
+
+  // [fill msg Header if necessary]
+
+  // [fill msg structures]
+  
+  // [fill srv structure and make request to the server]
+  
+  // [fill action structure and make request to the action server]
+
+  // [publish messages]
+
+  //unlock access to driver if previously blocked
+  this->driver_.unlock();
+}
+
+/*  [subscriber callbacks] */
+
+/*  [service callbacks] */
+
+/*  [action callbacks] */
+
+/*  [action requests] */
+
+void IrFeetSensorDriverNode::postNodeOpenHook(void)
+{
+}
+
+void IrFeetSensorDriverNode::addNodeDiagnostics(void)
+{
+}
+
+void IrFeetSensorDriverNode::addNodeOpenedTests(void)
+{
+}
+
+void IrFeetSensorDriverNode::addNodeStoppedTests(void)
+{
+}
+
+void IrFeetSensorDriverNode::addNodeRunningTests(void)
+{
+}
+
+void IrFeetSensorDriverNode::reconfigureNodeHook(int level)
+{
+}
+
+IrFeetSensorDriverNode::~IrFeetSensorDriverNode(void)
+{
+  // [free dynamic memory]
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+  return driver_base::main<IrFeetSensorDriverNode>(argc, argv, "ir_feet_sensor_driver_node");
+}