diff --git a/darwin_control/config/darwin_ceabot.yaml b/darwin_control/config/darwin_ceabot.yaml
index f3f88615112fe6a221468d2c8000930b68d67213..37ead9a1fc24a58ecc67a270435da0813f03991e 100644
--- a/darwin_control/config/darwin_ceabot.yaml
+++ b/darwin_control/config/darwin_ceabot.yaml
@@ -5,6 +5,8 @@ darwin:
     publish_rate: 50  
 
   darwin_controller:
+    left_sensor_foot_present: true
+    right_sensor_foot_present: true
     type: effort_controllers/DarwinController
     joints:
       - j_shoulder_pitch_r
diff --git a/darwin_controller/include/darwin_controller.h b/darwin_controller/include/darwin_controller.h
index 8b551ad04af2272188fdb3e89692a00fade41553..84345adc2844422d5605ac4c2cdaa40f469806e6 100644
--- a/darwin_controller/include/darwin_controller.h
+++ b/darwin_controller/include/darwin_controller.h
@@ -45,6 +45,7 @@
 #include <ros/node_handle.h>
 #include <std_msgs/Int8.h>
 #include <sensor_msgs/Imu.h>
+#include "sensor_msgs/Range.h"
 #include <geometry_msgs/Twist.h>
 #include <trajectory_msgs/JointTrajectoryPoint.h>
 #include <control_msgs/JointTrajectoryAction.h>
@@ -73,6 +74,7 @@
 #include <humanoid_common_msgs/get_smart_charger_config.h>
 #include <humanoid_common_msgs/set_smart_charger_config.h>
 #include <humanoid_common_msgs/smart_charger_data.h>
+#include <humanoid_common_msgs/ir_foot_data.h>
 #include <humanoid_modules/humanoid_module.h>
 
 #include <gazebo/transport/transport.hh>
@@ -197,8 +199,20 @@ namespace darwin_controller
       ros::Subscriber leds_subscriber_;
       void leds_callback(const humanoid_common_msgs::leds::ConstPtr& msg);
 
+      ros::Subscriber range_sub;
+      void range_callback(const sensor_msgs::Range::ConstPtr& msg);
+
       CMutex walk_access;
 
+      // feet sensors publihsers
+      ros::Publisher left_foot_sensor_data_publisher_;
+      humanoid_common_msgs::ir_foot_data left_foot_sensor_data_msg_;
+      bool left_sensor_foot_present;
+
+      ros::Publisher right_foot_sensor_data_publisher_;
+      humanoid_common_msgs::ir_foot_data right_foot_sensor_data_msg_;
+      bool right_sensor_foot_present;
+
       // fall detection publisher
       ros::Publisher fallen_state_publisher_;
       std_msgs::Int8 fallen_state_msg_;
diff --git a/darwin_controller/include/darwin_controller_impl.h b/darwin_controller/include/darwin_controller_impl.h
index 7a6ee9f7ec10decc8966dd13c36e5819f98e4baa..dd9af3acea864e7b73af67b57f2d06e89c0e708d 100644
--- a/darwin_controller/include/darwin_controller_impl.h
+++ b/darwin_controller/include/darwin_controller_impl.h
@@ -273,6 +273,50 @@ namespace darwin_controller
       // initialize publisher
       this->fallen_state_publisher_ = root_nh.advertise<std_msgs::Int8>("robot/fallen_state", 1);
 
+      this->left_sensor_foot_present=false;
+      controller_nh_.getParam("left_sensor_foot_present", this->left_sensor_foot_present);
+      ROS_DEBUG_STREAM_NAMED(name_, "left sensor foot is: " << this->left_sensor_foot_present);
+      if(this->left_sensor_foot_present)
+      {
+        this->left_foot_sensor_data_publisher_ = root_nh.advertise<humanoid_common_msgs::ir_foot_data>("sensors/left_foot_data",1);
+        this->left_foot_sensor_data_msg_.names.resize(10);
+        this->left_foot_sensor_data_msg_.names[0]="down_left_middle";
+        this->left_foot_sensor_data_msg_.names[1]="down_left_rear";
+        this->left_foot_sensor_data_msg_.names[2]="down_analog";
+        this->left_foot_sensor_data_msg_.names[3]="down_left_front";
+        this->left_foot_sensor_data_msg_.names[4]="down_right_rear";
+        this->left_foot_sensor_data_msg_.names[5]="down_right_middle";
+        this->left_foot_sensor_data_msg_.names[6]="down_right_front";
+        this->left_foot_sensor_data_msg_.names[7]="front_left";
+        this->left_foot_sensor_data_msg_.names[8]="front_right";
+        this->left_foot_sensor_data_msg_.names[9]="front_analog";
+        this->left_foot_sensor_data_msg_.voltages.resize(10);
+        this->left_foot_sensor_data_msg_.status.resize(10);
+      }
+
+      this->right_sensor_foot_present=false;
+      controller_nh_.getParam("right_sensor_foot_present", this->right_sensor_foot_present);
+      ROS_DEBUG_STREAM_NAMED(name_, "right sensor foot is: " << this->right_sensor_foot_present);
+      if(this->right_sensor_foot_present)
+      {
+        this->right_foot_sensor_data_publisher_ = root_nh.advertise<humanoid_common_msgs::ir_foot_data>("sensors/right_foot_data",1);
+        this->right_foot_sensor_data_msg_.names.resize(10);
+        this->right_foot_sensor_data_msg_.names[0]="down_left_middle";
+        this->right_foot_sensor_data_msg_.names[1]="down_left_rear";
+        this->right_foot_sensor_data_msg_.names[2]="down_analog";
+        this->right_foot_sensor_data_msg_.names[3]="down_left_front";
+        this->right_foot_sensor_data_msg_.names[4]="down_right_rear";
+        this->right_foot_sensor_data_msg_.names[5]="down_right_middle";
+        this->right_foot_sensor_data_msg_.names[6]="down_right_front";
+        this->right_foot_sensor_data_msg_.names[7]="front_left";
+        this->right_foot_sensor_data_msg_.names[8]="front_right";
+        this->right_foot_sensor_data_msg_.names[9]="front_analog";
+        this->right_foot_sensor_data_msg_.voltages.resize(10);
+        this->right_foot_sensor_data_msg_.status.resize(10);
+      }
+      if(this->right_sensor_foot_present || this->right_sensor_foot_present)
+        this->range_sub=root_nh.subscribe("sensors/range", 1, &DarwinController<HardwareInterface>::range_callback,this);
+
       // Controller name
       name_=getLeafNamespace(controller_nh_);
 
@@ -1075,7 +1119,6 @@ namespace darwin_controller
     }
 /************************************ get_tilt_pid *****************************************/
 
-/****************************************** imu *************************************************/
 /****************************************** imu *************************************************/
   template <class HardwareInterface>
     void DarwinController<HardwareInterface>::gyro_callback(const sensor_msgs::Imu::ConstPtr& msg)
@@ -1106,6 +1149,137 @@ namespace darwin_controller
     }
 /****************************************** imu *************************************************/
 
+  template <class HardwareInterface>
+    void DarwinController<HardwareInterface>::range_callback(const sensor_msgs::Range::ConstPtr& msg)
+    {
+      std::string frame_name;
+      unsigned char threshold;
+      static unsigned int remaining_left_sensors=10,remaining_right_sensors=10; 
+
+      if(msg->header.frame_id.find("/")==std::string::npos)
+        frame_name=msg->header.frame_id;
+      else
+        frame_name=msg->header.frame_id.substr(msg->header.frame_id.find("/")+1);
+
+      /* handle the feet sensors */
+      if(msg->range>(msg->max_range/2.0))
+        threshold=0x01;
+      else
+        threshold=0x00;
+      if(this->left_sensor_foot_present)
+      {
+        if(frame_name=="left_down_right_rear_ir_link")
+        {
+          this->left_foot_sensor_data_msg_.voltages[4]=msg->range;
+          this->left_foot_sensor_data_msg_.status[4]=threshold;
+          remaining_left_sensors--;
+        }
+        else if(frame_name=="left_down_right_middle_ir_link")
+        {
+          this->left_foot_sensor_data_msg_.voltages[5]=msg->range;
+          this->left_foot_sensor_data_msg_.status[5]=threshold;
+          remaining_left_sensors--;
+        }
+        else if(frame_name=="left_down_right_front_ir_link")
+        {
+          this->left_foot_sensor_data_msg_.voltages[6]=msg->range;
+          this->left_foot_sensor_data_msg_.status[6]=threshold;
+          remaining_left_sensors--;
+        }
+        else if(frame_name=="left_down_left_rear_ir_link")
+        {
+          this->left_foot_sensor_data_msg_.voltages[1]=msg->range;
+          this->left_foot_sensor_data_msg_.status[1]=threshold;
+          remaining_left_sensors--;
+        }
+        else if(frame_name=="left_down_left_middle_ir_link")
+        {
+          this->left_foot_sensor_data_msg_.voltages[0]=msg->range;
+          this->left_foot_sensor_data_msg_.status[0]=threshold;
+          remaining_left_sensors--;
+        }
+        else if(frame_name=="left_down_left_front_ir_link")
+        {
+          this->left_foot_sensor_data_msg_.voltages[3]=msg->range;
+          this->left_foot_sensor_data_msg_.status[3]=threshold;
+          remaining_left_sensors--;
+        }
+        else if(frame_name=="left_front_right_ir_link")
+        {
+          this->left_foot_sensor_data_msg_.voltages[8]=msg->range;
+          this->left_foot_sensor_data_msg_.status[8]=threshold;
+          remaining_left_sensors--;
+        }
+        else if(frame_name=="left_front_left_ir_link")
+        {
+          this->left_foot_sensor_data_msg_.voltages[7]=msg->range;
+          this->left_foot_sensor_data_msg_.status[7]=threshold;
+          remaining_left_sensors--;
+        }
+        if(remaining_left_sensors==0)
+        {
+          remaining_left_sensors=10;
+          this->left_foot_sensor_data_publisher_.publish(this->left_foot_sensor_data_msg_); 
+        }
+      }
+      if(this->right_sensor_foot_present)
+      {
+        if(frame_name=="right_down_right_rear_ir_link")
+        {
+          this->right_foot_sensor_data_msg_.voltages[4]=msg->range;
+          this->right_foot_sensor_data_msg_.status[4]=threshold;
+          remaining_right_sensors--;
+        }
+        else if(frame_name=="right_down_right_middle_ir_link")
+        {
+          this->right_foot_sensor_data_msg_.voltages[5]=msg->range;
+          this->right_foot_sensor_data_msg_.status[5]=threshold;
+          remaining_right_sensors--;
+        }
+        else if(frame_name=="right_down_right_front_ir_link")
+        {
+          this->right_foot_sensor_data_msg_.voltages[6]=msg->range;
+          this->right_foot_sensor_data_msg_.status[6]=threshold;
+          remaining_right_sensors--;
+        }
+        else if(frame_name=="right_down_left_rear_ir_link")
+        {
+          this->right_foot_sensor_data_msg_.voltages[1]=msg->range;
+          this->right_foot_sensor_data_msg_.status[1]=threshold;
+          remaining_right_sensors--;
+        }
+        else if(frame_name=="right_down_left_middle_ir_link")
+        {
+          this->right_foot_sensor_data_msg_.voltages[0]=msg->range;
+          this->right_foot_sensor_data_msg_.status[0]=threshold;
+          remaining_right_sensors--;
+        }
+        else if(frame_name=="right_down_left_front_ir_link")
+        {
+          this->right_foot_sensor_data_msg_.voltages[3]=msg->range;
+          this->right_foot_sensor_data_msg_.status[3]=threshold;
+          remaining_right_sensors--;
+        }
+        else if(frame_name=="right_front_right_ir_link")
+        {
+          this->right_foot_sensor_data_msg_.voltages[8]=msg->range;
+          this->right_foot_sensor_data_msg_.status[8]=threshold;
+          remaining_right_sensors--;
+        }
+        else if(frame_name=="right_front_left_ir_link")
+        {
+          this->right_foot_sensor_data_msg_.voltages[7]=msg->range;
+          this->right_foot_sensor_data_msg_.status[7]=threshold;
+          remaining_right_sensors--;
+        }
+        if(remaining_right_sensors==0)
+        {
+          remaining_right_sensors=10;
+          this->right_foot_sensor_data_publisher_.publish(this->right_foot_sensor_data_msg_); 
+        }
+      }
+    }
+
 /****************************************** cmd_vel *********************************************/
   template <class HardwareInterface>
     void DarwinController<HardwareInterface>::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)