diff --git a/README.md b/README.md
index 8405d9998e4b0f33f4ed653741248a0b0a226ec1..3d676ff7ffb8d7c4f1ff28c12cc0a2eb7e012f2c 100644
--- a/README.md
+++ b/README.md
@@ -17,6 +17,7 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
 
 # ROS Interface
 ### Topic publishers
+  - ~**relocalization** (iri_collision_manager/relocalization.msg)
   - ~**collisions** (iri_collision_manager/collision.msg)
   - ~**debug_rear_after_icp_scan** (sensor_msgs/LaserScan.msg)
   - ~**debug_front_after_icp_scan** (sensor_msgs/LaserScan.msg)
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index 19e6ba41d81ef76c01436eaa819cfe7e02927221..e056f513aaaebed1475038bd9ef3d21181a7ad11 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -32,6 +32,7 @@
 #include <iri_ros_tools/watchdog.h>
 
 // [publisher subscriber headers]
+#include <iri_collision_manager/relocalization.h>
 #include <iri_collision_manager/collision.h>
 #include <nav_msgs/Odometry.h>
 #include <tf2_ros/transform_broadcaster.h>
@@ -90,10 +91,10 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     bool tf_base_link_imu_loaded_; ///< Flag to know that tf_base_link_imu has been loaded before.
     Eigen::Transform<double,3,Eigen::Affine> tf_base_link_imu_; ///< Transform from base_link frame to IMU frame.
     bool tf_base_link_front_laser_loaded_; ///< Flag to know that tf_base_link_front_laser has been loaded before.
-    tf2::Transform tf_blink_front_laser; ///< Transform from base_link to front laser.
+    tf2::Transform tf_blink_front_laser_; ///< Transform from base_link to front laser.
     bool z_axis_blink_front_laser_inv_; ///< Flag to know if the z axis of base link and front laser have the same orientation.
     bool tf_base_link_rear_laser_loaded_; ///< Flag to know that tf_base_link_rear_laser has been loaded before.
-    tf2::Transform tf_blink_rear_laser; ///< Transform from base_link to rear laser.
+    tf2::Transform tf_blink_rear_laser_; ///< Transform from base_link to rear laser.
     bool z_axis_blink_rear_laser_inv_; ///< Flag to know if the z axis of base link and rear laser have the same orientation.
     bool in_collision_; ///< Flag to know that we are receiving collision meassures.
     ros::Time collision_start_stamp_; ///< Time stamp of a collision initial detection.
@@ -118,6 +119,9 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     CROSWatchdog rear_laser_watchdog_; ///< Rear laser callback watchdog.
 
     // [publisher attributes]
+    ros::Publisher relocalization_publisher_;
+    iri_collision_manager::relocalization relocalization_msg_;
+
     ros::Publisher collisions_publisher_;
     iri_collision_manager::collision collision_msg_;
 
@@ -390,6 +394,25 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
      */
     bool check_odometry_diff(const Eigen::Vector3d& _icp_odometry, Eigen::Vector3d& _icp_odometry_diff);
 
+    /**
+     * \brief Function to get the new robot's base_link pose from ICP result.
+     * 
+     * It checks that ICP result is good enough and calculates the pose.
+     * 
+     * \param _new_laser_pose New laser pose from ICP.
+     * 
+     * \param _covariances Laser pose covariances from ICP.
+     * 
+     * \param _blink_icp ICP on base link frame.
+     * 
+     * \param _blink_icp_cov ICP covariances on base link frame.
+     * 
+     * \param _front To select the laser.
+     * 
+     * \return True if succeeded.
+     */
+    bool rotate_icp_to_blink(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _blink_icp, Eigen::Matrix3d& _blink_icp_cov, const bool _front);
+
    /**
     * \brief main node thread
     *
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 4ff10fd0d20789e8d1199fae9c01df52c41f2575..db49eddd68f4cd51cdb5bc5d1fb80f0dcdbc3f08 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -89,8 +89,10 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
     this->debug_front_icp_pose_msg_.pose.covariance[i] = 0.0;
 
   this->collision_msg_.header.frame_id = "iri_collision_manager";
+  this->relocalization_msg_.header.frame_id = "iri_collision_manager";
 
   // [init publishers]
+  this->relocalization_publisher_ = this->private_node_handle_.advertise<iri_collision_manager::relocalization>("relocalization", 1);
   this->collisions_publisher_ = this->private_node_handle_.advertise<iri_collision_manager::collision>("collisions", 1);
   this->debug_rear_after_icp_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_rear_after_icp_scan", 1);
   this->debug_front_after_icp_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_front_after_icp_scan", 1);
@@ -144,8 +146,8 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   this->rear_laser_scan_mutex_enter();
   this->front_laser_scan_mutex_enter();
   this->odom_mutex_enter();
-  Eigen::Vector3d blink_acc, blink_ang_vel, blink_pose, new_laser_pose, icp_odometry, icp_odometry_diff, diff_wheel_icp_odom;
-  Eigen::Matrix3d blink_cov, covariances, icp_odom_cov;
+  Eigen::Vector3d blink_acc, blink_ang_vel, blink_pose, new_laser_pose, icp_odometry, icp_odometry_diff, diff_wheel_icp_odom, blink_icp;
+  Eigen::Matrix3d blink_cov, covariances, icp_odom_cov, blink_icp_cov;
   bool icp_succedded, icp_odom_calculated, icp_blink_pose_calculated, icp_is_collision;
 
   if (this->new_imu_input_)
@@ -252,6 +254,25 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                     ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Not enough points or too much error on front ICP. Not valid to check if is a collision");
                     icp_is_collision = false;
                   }
+                  else
+                  {
+                    this->relocalization_msg_.front_icp_check = true;
+                    this->relocalization_msg_.front_blink_pose.x = blink_pose(0);
+                    this->relocalization_msg_.front_blink_pose.y = blink_pose(1);
+                    this->relocalization_msg_.front_blink_pose.theta = blink_pose(2);
+                    for (unsigned int i=0; i<9; i++)
+                      this->relocalization_msg_.front_pose_covariance[i] = blink_cov(i);
+                    if (rotate_icp_to_blink(new_laser_pose, covariances, blink_icp, blink_icp_cov, true))
+                    {
+                      this->relocalization_msg_.front_icp_odom.x = blink_icp(0);
+                      this->relocalization_msg_.front_icp_odom.y = blink_icp(1);
+                      this->relocalization_msg_.front_icp_odom.theta = blink_icp(2);
+                      for (unsigned int i=0; i<9; i++)
+                        this->relocalization_msg_.front_odom_covariance[i] = blink_icp_cov(i);
+                    }
+                    else
+                      this->relocalization_msg_.front_icp_check = false;
+                  }
                 }
                 else
                 {
@@ -350,6 +371,25 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                     ROS_WARN("CollisionManagerAlgNode::mainNodeThread -> Not enough points or too much error on rear ICP. Not valid to check if is a collision");
                     icp_is_collision = false;
                   }
+                  else
+                  {
+                    this->relocalization_msg_.rear_icp_check = true;
+                    this->relocalization_msg_.rear_blink_pose.x = blink_pose(0);
+                    this->relocalization_msg_.rear_blink_pose.y = blink_pose(1);
+                    this->relocalization_msg_.rear_blink_pose.theta = blink_pose(2);
+                    for (unsigned int i=0; i<9; i++)
+                      this->relocalization_msg_.rear_pose_covariance[i] = blink_cov(i);
+                    if (rotate_icp_to_blink(new_laser_pose, covariances, blink_icp, blink_icp_cov, false))
+                    {
+                      this->relocalization_msg_.rear_icp_odom.x = blink_icp(0);
+                      this->relocalization_msg_.rear_icp_odom.y = blink_icp(1);
+                      this->relocalization_msg_.rear_icp_odom.theta = blink_icp(2);
+                      for (unsigned int i=0; i<9; i++)
+                        this->relocalization_msg_.rear_odom_covariance[i] = blink_icp_cov(i);
+                    }
+                    else
+                      this->relocalization_msg_.rear_icp_check = false;
+                  }
                 }
                 else
                 {
@@ -397,6 +437,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
 
           //publish collision.
           this->collisions_publisher_.publish(this->collision_msg_);
+          this->relocalization_publisher_.publish(this->relocalization_msg_);
         }
         else 
         {
@@ -451,18 +492,29 @@ void CollisionManagerAlgNode::mainNodeThread(void)
 
         //update collision msg data
         this->collision_msg_.header.stamp = this->imu_stamp_;
+
+        //update relocalization msg
+        this->relocalization_msg_.header.stamp = this->imu_stamp_;
+        this->relocalization_msg_.front_icp_check = false;
+        this->relocalization_msg_.rear_icp_check = false;
       }
     }
     this->new_imu_input_ = false;
   }
 
   // [fill msg structures]
+  // Initialize the topic message structure
+  //this->relocalization_relocalization_msg_.data = my_var;
+
   
   // [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->relocalization_publisher_.publish(this->relocalization_relocalization_msg_);
+
 
   this->odom_mutex_exit();
   this->front_laser_scan_mutex_exit();
@@ -690,7 +742,7 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
   tf_laser_new_laser = tf2::Transform(q, tf2::Vector3(_new_laser_pose(0), _new_laser_pose(1), 0.0));
   tf_map_new_laser = tf_map_laser*tf_laser_new_laser;
 
-  tf_map_new_blink = tf_map_new_laser*(_front? this->tf_blink_front_laser: this->tf_blink_rear_laser).inverse();
+  tf_map_new_blink = tf_map_new_laser*(_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).inverse();
   _blink_pose << tf_map_new_blink.getOrigin().x(), tf_map_new_blink.getOrigin().y(), tf2::getYaw(tf_map_new_blink.getRotation());
 
   Eigen::Quaternion<double> rot(tf_map_laser.getRotation().w(), tf_map_laser.getRotation().x(), tf_map_laser.getRotation().y(), tf_map_laser.getRotation().z());
@@ -773,7 +825,7 @@ bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser
   tf_laser_new_laser = tf2::Transform(q, tf2::Vector3(_new_laser_pose(0), _new_laser_pose(1), 0.0));
   tf_odom_new_laser = tf_odom_laser*tf_laser_new_laser;
 
-  tf_odom_new_blink = tf_odom_new_laser*(_front? this->tf_blink_front_laser: this->tf_blink_rear_laser).inverse();
+  tf_odom_new_blink = tf_odom_new_laser*(_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).inverse();
   _odom << tf_odom_new_blink.getOrigin().x(), tf_odom_new_blink.getOrigin().y(), tf2::getYaw(tf_odom_new_blink.getRotation());
 
   Eigen::Quaternion<double> rot(tf_odom_laser.getRotation().w(), tf_odom_laser.getRotation().x(), tf_odom_laser.getRotation().y(), tf_odom_laser.getRotation().z());
@@ -781,8 +833,8 @@ bool CollisionManagerAlgNode::get_icp_odometry(const Eigen::Vector3d& _new_laser
   Eigen::Transform<double,3,Eigen::Affine> odom_laser_rot(rot);
   _odom_cov = odom_laser_rot.rotation()*_covariances*odom_laser_rot.rotation().transpose();
 
-  for (unsigned int i=0; i<9; i++)
-    _odom_cov(i) = std::fabs(_odom_cov(i))*(_covariances(i) < 0? -1: 1);
+  // for (unsigned int i=0; i<9; i++)
+  //   _odom_cov(i) = std::fabs(_odom_cov(i))*(_covariances(i) < 0? -1: 1);
 
   _odom_diff = _odom - old_odom;
   while (_odom_diff(2) >= M_PI)
@@ -807,7 +859,7 @@ bool CollisionManagerAlgNode::load_tf_blink_laser(bool _front)
   else
   {
     tf_aux = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, (_front? this->front_laser_frame_: this->rear_laser_frame_), this->imu_stamp_);
-    tf2::fromMsg(tf_aux.transform, (_front? this->tf_blink_front_laser: this->tf_blink_rear_laser));
+    tf2::fromMsg(tf_aux.transform, (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_));
     (_front? this->tf_base_link_front_laser_loaded_: this->tf_base_link_rear_laser_loaded_) = true;
 
     // check if z axis from base link and laser have the same orientation
@@ -851,6 +903,27 @@ bool CollisionManagerAlgNode::check_odometry_diff(const Eigen::Vector3d& _icp_od
   return (odom_check(0) || odom_check(1) || odom_check(2));
 }
 
+bool CollisionManagerAlgNode::rotate_icp_to_blink(const Eigen::Vector3d& _new_laser_pose, const Eigen::Matrix3d& _covariances, Eigen::Vector3d& _blink_icp, Eigen::Matrix3d& _blink_icp_cov, const bool _front)
+{
+  if (!load_tf_blink_laser(_front))
+    return false;
+
+  Eigen::Quaternion<double> rot((_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().w(), 
+                                (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().x(),
+                                (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().y(), 
+                                (_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation().z());
+  Eigen::Transform<double,3,Eigen::Affine> blink_laser_rot(rot);
+
+  Eigen::Vector3d aux;
+  aux << _new_laser_pose(0), _new_laser_pose(1), 0.0;
+  _blink_icp = blink_laser_rot*aux;
+  _blink_icp(2) = _new_laser_pose(2) + tf2::getYaw((_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation());
+  _blink_icp_cov = blink_laser_rot.rotation()*_covariances*blink_laser_rot.rotation().transpose();
+  ROS_INFO_STREAM("input " << _new_laser_pose);
+  ROS_INFO_STREAM("output " << _blink_icp);
+  return true;
+}
+
 /*  [subscriber callbacks] */
 void CollisionManagerAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
 {