diff --git a/ir_foot_sensor/CMakeLists.txt b/ir_foot_sensor/CMakeLists.txt
index 63f507aa83f7b60a1326ac88d037e6c4f39cd6b3..ca5fdd2b28c9fa5819df9bb179ec94d90aee7e57 100644
--- a/ir_foot_sensor/CMakeLists.txt
+++ b/ir_foot_sensor/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_driver humanoid_common_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver humanoid_common_msgs nodelet)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -53,7 +53,7 @@ find_package(ir_feet REQUIRED)
 # ******************************************************************** 
 #                 Add the dynamic reconfigure file 
 # ******************************************************************** 
-generate_dynamic_reconfigure_options(cfg/IrFeetSensor.cfg)
+generate_dynamic_reconfigure_options(cfg/IrFootSensor.cfg)
 
 # ******************************************************************** 
 #                 Add run time dependencies here
@@ -64,7 +64,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_driver humanoid_common_msgs
+ CATKIN_DEPENDS iri_base_driver humanoid_common_msgs nodelet
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
@@ -89,6 +89,7 @@ include_directories(${ir_feet_INCLUDE_DIR})
 ## Declare a cpp library
 # add_library(${PROJECT_NAME} <list of source files>)
 # add_library(${PROJECT_NAME} <list of source files>)
+add_library(${PROJECT_NAME}_nodelet src/ir_foot_sensor_driver.cpp src/ir_foot_sensor_driver_node.cpp)
 
 ## Declare a cpp executable
 add_executable(${PROJECT_NAME} src/ir_foot_sensor_driver.cpp src/ir_foot_sensor_driver_node.cpp)
@@ -103,6 +104,11 @@ target_link_libraries(${PROJECT_NAME} ${dynamixel_LIBRARY})
 target_link_libraries(${PROJECT_NAME} ${ir_feet_LIBRARY})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
 
+target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_nodelet ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${comm_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${dynamixel_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${ir_feet_LIBRARY})
 # ******************************************************************** 
 #               Add message headers dependencies 
 # ******************************************************************** 
diff --git a/ir_foot_sensor/include/ir_foot_sensor_driver.h b/ir_foot_sensor/include/ir_foot_sensor_driver.h
index 60c36c70497a8ac1b056ea007bc4e4bfff04d738..abed4b7efbe8c85749eca95cc3cd4539d1a2821e 100644
--- a/ir_foot_sensor/include/ir_foot_sensor_driver.h
+++ b/ir_foot_sensor/include/ir_foot_sensor_driver.h
@@ -55,7 +55,7 @@ class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
 {
   private:
     // private attributes and methods
-    CIRFoot<CDynamixelServerSerial> *device;
+    CIRFeet<CDynamixelServerSerial> *device;
   public:
    /**
     * \brief define config type
diff --git a/ir_foot_sensor/include/ir_foot_sensor_driver_node.h b/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
index 186a6ef9b3bc5023343df79ae9a295bc63813935..b93c024f8c14effa535cb3a55bacebb2e00b877f 100644
--- a/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
+++ b/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
@@ -182,4 +182,29 @@ class IrFootSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFootS
 
 };
 
+#include "nodelet/nodelet.h"
+
+class IrFootSensorDriverNodelet : public nodelet::Nodelet
+{
+  private:
+    IrFootSensorDriverNode *node;
+    virtual void onInit();// initialization function
+    // thread attributes
+    CThreadServer *thread_server;
+    std::string spin_thread_id;
+  protected:
+    static void *spin_thread(void *param);
+  public:
+    IrFootSensorDriverNodelet();
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~IrFootSensorDriverNodelet();
+};
+
+
 #endif
diff --git a/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml b/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml
new file mode 100755
index 0000000000000000000000000000000000000000..b8c9d6193ba2a82bb7400d270c2fe83c3a835711
--- /dev/null
+++ b/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libir_foot_sensor_nodelet">
+  <class name="ir_foot_sensor/IrFootSensorDriverNodelet" type="IrFootSensorDriverNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelete for the IR foot sensor driver ROS wrapper 
+    </description>
+  </class>
+</library>
diff --git a/ir_foot_sensor/launch/darwin_left_foot.launch b/ir_foot_sensor/launch/darwin_left_foot.launch
new file mode 100644
index 0000000000000000000000000000000000000000..61d8490bbcf62b782b0ce78dd37caab56b9537c2
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_left_foot.launch
@@ -0,0 +1,13 @@
+<!-- -->
+<launch>
+  <node name="darwin_left_foot" 
+        pkg="ir_foot_sensor" 
+        type="ir_foot_sensor" 
+        output="screen">
+    <param name="dyn_serial" value="A4008atn"/>
+    <param name="dyn_baudrate" value="1000000"/>
+    <param name="ir_foot_id" value="1"/>
+    <remap from="/darwin_left_foot/sensor_data"
+             to="/darwin/sensors/left_foot_data"/>
+  </node>
+</launch>
diff --git a/ir_foot_sensor/launch/darwin_right_foot.launch b/ir_foot_sensor/launch/darwin_right_foot.launch
new file mode 100644
index 0000000000000000000000000000000000000000..df674f2e27fbaca24d9a718ab36215795c18c1f7
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_right_foot.launch
@@ -0,0 +1,13 @@
+<!-- -->
+<launch>
+  <node name="darwin_right_foot" 
+        pkg="ir_foot_sensor" 
+        type="ir_foot_sensor" 
+        output="screen">
+    <param name="dyn_serial" value="A4008atn"/>
+    <param name="dyn_baudrate" value="1000000"/>
+    <param name="ir_foot_id" value="2"/>
+    <remap from="/darwin_right_foot/sensor_data"
+             to="/darwin/sensors/right_foot_data"/>
+  </node>
+</launch>
diff --git a/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch b/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
new file mode 100755
index 0000000000000000000000000000000000000000..210fb48de7eb1219ee26db4962c9a16ba7cf370a
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
@@ -0,0 +1,36 @@
+<!-- -->
+<launch>
+  <node pkg="nodelet"
+        type="nodelet" 
+        name="dynamixel_devices"
+        args="manager"
+        output="screen">
+  </node>
+
+  <!-- left foot -->
+  <node pkg="nodelet" 
+        type="nodelet" 
+        name="darwin_left_foot" 
+        args="load ir_foot_sensor/IrFootSensorDriverNodelet dynamixel_devices"
+        output="screen">
+    <param name="dyn_serial" value="A4008atn"/>
+    <param name="dyn_baudrate" value="1000000"/>
+    <param name="ir_foot_id" value="1"/>
+    <remap from="/darwin_left_foot/sensor_data"
+             to="/darwin/sensors/left_foot_data"/>
+  </node>
+
+  <!-- rught foot -->
+  <node pkg="nodelet" 
+        type="nodelet" 
+        name="darwin_right_foot" 
+        args="load ir_foot_sensor/IrFootSensorDriverNodelet dynamixel_devices"
+        output="screen">
+    <param name="dyn_serial" value="A4008atn"/>
+    <param name="dyn_baudrate" value="1000000"/>
+    <param name="ir_foot_id" value="2"/>
+    <remap from="/darwin_right_foot/sensor_data"
+             to="/darwin/sensors/right_foot_data"/>
+  </node>
+
+</launch>
diff --git a/ir_foot_sensor/package.xml b/ir_foot_sensor/package.xml
index dd9ede5c0c1a5ee0e70c8bb6a4506d89c9304275..b384adb3b9bde53c329e9386a90e68ce1ac7c459 100644
--- a/ir_foot_sensor/package.xml
+++ b/ir_foot_sensor/package.xml
@@ -42,13 +42,14 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>iri_base_driver</build_depend>
   <build_depend>humanoid_common_msgs</build_depend>
+  <build_depend>nodelet</build_depend>
   <run_depend>iri_base_driver</run_depend>
   <run_depend>humanoid_common_msgs</run_depend>
+  <run_depend>nodelet</run_depend>
 
-
-  <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
+    <nodelet plugin="${prefix}/ir_foot_sensor_nodelet_plugin.xml" />
   </export>
+
 </package>
diff --git a/ir_foot_sensor/src/ir_foot_sensor_driver.cpp b/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
index a5ab37b9eb30051abf1080257da70265cbdbc578..7d71cff775c6dfa1ba453ca15cd01c7020f0737d 100644
--- a/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
+++ b/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
@@ -16,8 +16,8 @@ bool IrFootSensorDriver::openDriver(void)
       delete this->device;
       this->device=NULL;
     }
-    name << foot << "_" << this->config_.ir_foot_id;
-    this->device=new CIRFoot<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
+    name << "foot_" << this->config_.ir_foot_id;
+    this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
 
     return true;
   }catch(CException &e){
diff --git a/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp b/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
index adebbbb46598084db4ff784106a26c46bff29f71..a3773e6b8dfeb53db1de6a05f76e93cce81c3f02 100644
--- a/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
+++ b/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
@@ -4,7 +4,7 @@ IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
   iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>(nh)
 {
   //init class attributes if necessary
-  //this->loop_rate_ = 2;//in [Hz]
+  this->loop_rate_ = 10;//in [Hz]
 
   // [init publishers]
   this->sensor_data_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
@@ -18,28 +18,49 @@ IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
   // [init action servers]
   
   // [init action clients]
+  this->sensor_data_ir_foot_data_msg_.names.resize(10);
+  this->sensor_data_ir_foot_data_msg_.names[0]="down_left_middle";
+  this->sensor_data_ir_foot_data_msg_.names[1]="down_left_rear";
+  this->sensor_data_ir_foot_data_msg_.names[2]="down_analog";
+  this->sensor_data_ir_foot_data_msg_.names[3]="down_left_front";
+  this->sensor_data_ir_foot_data_msg_.names[4]="down_right_rear";
+  this->sensor_data_ir_foot_data_msg_.names[5]="down_right_middle";
+  this->sensor_data_ir_foot_data_msg_.names[6]="down_right_front";
+  this->sensor_data_ir_foot_data_msg_.names[7]="front_left";
+  this->sensor_data_ir_foot_data_msg_.names[8]="front_right";
+  this->sensor_data_ir_foot_data_msg_.names[9]="front_analog";
 }
 
 void IrFootSensorDriverNode::mainNodeThread(void)
 {
+  std::vector<bool> status;
   //lock access to driver if necessary
   this->driver_.lock();
 
   // [fill msg Header if necessary]
 
   // [fill msg structures]
-  // Initialize the topic message structure
-  //this->sensor_data_ir_foot_data_msg_.data = my_var;
-
+  if(this->driver_.isRunning())
+  {
+    this->sensor_data_ir_foot_data_msg_.header.stamp=ros::Time::now();
+    this->sensor_data_ir_foot_data_msg_.voltages=this->driver_.get_all_sensor_voltages(); 
+    status=this->driver_.get_all_sensor_status(); 
+    this->sensor_data_ir_foot_data_msg_.status.resize(status.size()); 
+    for(unsigned int i=0;i<status.size();i++)
+    {
+      if(status[i])
+        this->sensor_data_ir_foot_data_msg_.status[i]=0x01;
+      else
+        this->sensor_data_ir_foot_data_msg_.status[i]=0x00;
+    }
+    this->sensor_data_publisher_.publish(this->sensor_data_ir_foot_data_msg_);
+  }
   
   // [fill srv structure and make request to the server]
   
   // [fill action structure and make request to the action server]
 
   // [publish messages]
-  // Uncomment the following line to publish the topic message
-  //this->sensor_data_publisher_.publish(this->sensor_data_ir_foot_data_msg_);
-
 
   //unlock access to driver if previously blocked
   this->driver_.unlock();
@@ -87,3 +108,47 @@ int main(int argc,char *argv[])
 {
   return driver_base::main<IrFootSensorDriverNode>(argc, argv, "ir_foot_sensor_driver_node");
 }
+
+#include <pluginlib/class_list_macros.h>
+
+IrFootSensorDriverNodelet::IrFootSensorDriverNodelet()
+{
+  this->node=NULL;
+}
+
+IrFootSensorDriverNodelet::~IrFootSensorDriverNodelet(void)
+{
+  // kill the thread
+  this->thread_server->kill_thread(this->spin_thread_id);
+  this->thread_server->detach_thread(this->spin_thread_id);
+  this->thread_server->delete_thread(this->spin_thread_id);
+  this->spin_thread_id="";
+  // [free dynamic memory]
+  if(this->node!=NULL)
+    delete this->node;
+}
+
+void IrFootSensorDriverNodelet::onInit()
+{
+  // initialize the driver node
+  this->node=new IrFootSensorDriverNode(getPrivateNodeHandle());
+  // initialize the thread
+  this->thread_server=CThreadServer::instance();
+  this->spin_thread_id=getName() + "_firewire_nodelet_spin";
+  this->thread_server->create_thread(this->spin_thread_id);
+  this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
+  // start the spin thread
+  this->thread_server->start_thread(this->spin_thread_id);
+}
+
+void *IrFootSensorDriverNodelet::spin_thread(void *param)
+{
+  IrFootSensorDriverNodelet *nodelet=(IrFootSensorDriverNodelet *)param;
+
+  nodelet->node->spin();
+
+  pthread_exit(NULL);
+}
+
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(ir_foot_sensor, IrFootSensorDriverNodelet, IrFootSensorDriverNodelet, nodelet::Nodelet);