diff --git a/README.md b/README.md
index 4f96e4dca8f7a0e5321ebbf781c27d4f5b7a18ec..b98464c052acfdc1254208bd4c5fd705eaea1e08 100644
--- a/README.md
+++ b/README.md
@@ -10,6 +10,9 @@ It also provides a [default bno055 calibration file](./calibration/bno055.cal).
 
 # ROS Interface
 ### Topic publishers
+  - ~**debug_rear_after_icp_scan** (sensor_msgs/LaserScan.msg)
+  - ~**debug_front_after_icp_scan** (sensor_msgs/LaserScan.msg)
+  - /**tf** (tf/tfMessage)
   - ~**debug_rear_icp_pose** (geometry_msgs/PoseWithCovariance.msg)
   - ~**debug_front_icp_pose** (geometry_msgs/PoseWithCovariance.msg)
   - ~**debug_rear_icp_last_scan** (sensor_msgs/LaserScan.msg): Debug topic to visulize rear ICP matching laser scan.
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index 1993cbd40d1340aa5899bbdccae351676df9ab47..b690da1aa0c5cb22fd823311cd99a9d8c3aaad30 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -31,6 +31,7 @@
 #include <tf2/utils.h>
 
 // [publisher subscriber headers]
+#include <tf2_ros/transform_broadcaster.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <sensor_msgs/LaserScan.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -88,6 +89,15 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
     bool rear_icp_succedded_; ///< Boolean to know that ICP service was called withs success.
 
     // [publisher attributes]
+    ros::Publisher debug_rear_after_icp_scan_publisher_;
+    sensor_msgs::LaserScan debug_rear_after_icp_scan_msg_;
+
+    ros::Publisher debug_front_after_icp_scan_publisher_;
+    sensor_msgs::LaserScan debug_front_after_icp_scan_msg_;
+
+    tf2_ros::TransformBroadcaster tf2_broadcaster;
+    geometry_msgs::TransformStamped transform_msg;
+
     ros::Publisher debug_rear_icp_pose_publisher_;
     geometry_msgs::PoseWithCovarianceStamped debug_rear_icp_pose_msg_;
 
diff --git a/rviz/dogs.rviz b/rviz/dogs.rviz
index e6c138beb56928c9189c11310a425029d57d93ad..a870bf98bd9c035b30aea54fb90fd2319c79b915 100644
--- a/rviz/dogs.rviz
+++ b/rviz/dogs.rviz
@@ -10,6 +10,7 @@ Panels:
         - /LandmarksRobotPose1/Covariance1
         - /LandmarksRobotPose1/Covariance1/Position1
         - /LandmarksRobotPose1/Covariance1/Orientation1
+        - /Front_icp1/front_after_icp1/Status1
       Splitter Ratio: 0.5
     Tree Height: 728
   - Class: rviz/Selection
@@ -53,6 +54,10 @@ Visualization Manager:
           Value: true
         map:
           Value: true
+        new_front_laser:
+          Value: true
+        new_rear_laser:
+          Value: true
         odom:
           Value: true
       Marker Scale: 1
@@ -65,9 +70,11 @@ Visualization Manager:
           odom:
             base_link:
               base_laser_front_link:
-                {}
+                new_front_laser:
+                  {}
               base_laser_rear_link:
-                {}
+                new_rear_laser:
+                  {}
               imu_bno055:
                 {}
       Update Interval: 0
@@ -266,7 +273,7 @@ Visualization Manager:
           Scale: 1
           Value: true
         Value: true
-      Enabled: true
+      Enabled: false
       Head Length: 0.30000001192092896
       Head Radius: 0.10000000149011612
       Name: amclPose
@@ -275,7 +282,7 @@ Visualization Manager:
       Shape: Arrow
       Topic: /amcl_pose
       Unreliable: false
-      Value: true
+      Value: false
     - Alpha: 1
       Arrow Length: 0.30000001192092896
       Axes Length: 0.30000001192092896
@@ -346,7 +353,7 @@ Visualization Manager:
           Scale: 1
           Value: true
         Value: true
-      Enabled: true
+      Enabled: false
       Head Length: 0.30000001192092896
       Head Radius: 0.10000000149011612
       Name: LandmarksRobotPose
@@ -355,7 +362,7 @@ Visualization Manager:
       Shape: Arrow
       Topic: /iri_landmarks_tracker/landmarks_localization_pose
       Unreliable: false
-      Value: true
+      Value: false
     - Alpha: 0.5
       Axes Length: 1
       Axes Radius: 0.10000000149011612
@@ -376,7 +383,7 @@ Visualization Manager:
           Scale: 1
           Value: true
         Value: true
-      Enabled: true
+      Enabled: false
       Head Length: 0.30000001192092896
       Head Radius: 0.10000000149011612
       Name: FusedPose
@@ -385,7 +392,7 @@ Visualization Manager:
       Shape: Arrow
       Topic: /iri_localization_fusion/fused_pose
       Unreliable: false
-      Value: true
+      Value: false
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /iri_localization_fusion/fusion_traj_vis
@@ -493,6 +500,34 @@ Visualization Manager:
           Topic: /iri_collision_manager/debug_front_icp_pose
           Unreliable: false
           Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/LaserScan
+          Color: 252; 233; 79
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 252; 233; 79
+          Min Color: 0; 0; 0
+          Name: front_after_icp
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 7
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /iri_collision_manager/debug_front_after_icp_scan
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
       Enabled: true
       Name: Front_icp
     - Class: rviz/Group
@@ -583,6 +618,34 @@ Visualization Manager:
           Topic: /iri_collision_manager/debug_rear_icp_pose
           Unreliable: false
           Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/LaserScan
+          Color: 173; 127; 168
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 173; 127; 168
+          Min Color: 0; 0; 0
+          Name: rear_after_icp
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 7
+          Size (m): 0.009999999776482582
+          Style: Points
+          Topic: /iri_collision_manager/debug_rear_after_icp_scan
+          Unreliable: false
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
       Enabled: true
       Name: Rear_icp
   Enabled: true
@@ -622,11 +685,11 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Scale: 48.28645324707031
+      Scale: 43.86905288696289
       Target Frame: base_link
       Value: TopDownOrtho (rviz)
-      X: 0.663458526134491
-      Y: -0.6147326827049255
+      X: 1.2449862957000732
+      Y: -0.583761990070343
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 1d163d6867f6013004fc8e7386cf4f53b45607dc..27423918f628193635de175ed82800bb1dd14224 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -50,6 +50,8 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
     this->debug_front_icp_pose_msg_.pose.covariance[i] = 0.0;
 
   // [init publishers]
+  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);
   this->debug_rear_icp_pose_publisher_ = this->private_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("debug_rear_icp_pose", 1);
   this->debug_front_icp_pose_publisher_ = this->private_node_handle_.advertise<geometry_msgs::PoseWithCovarianceStamped>("debug_front_icp_pose", 1);
   this->debug_rear_icp_last_scan_publisher_ = this->private_node_handle_.advertise<sensor_msgs::LaserScan>("debug_rear_icp_last_scan", 1);
@@ -148,6 +150,22 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                                   front_icp_srv_.response.covariance[3], front_icp_srv_.response.covariance[4], front_icp_srv_.response.covariance[5],
                                   front_icp_srv_.response.covariance[6], front_icp_srv_.response.covariance[7], front_icp_srv_.response.covariance[8];
                   this->front_icp_succedded_ = true;
+
+                  this->transform_msg.header.frame_id = this->front_laser_frame_;
+                  this->transform_msg.child_frame_id = "new_front_laser";
+                  this->transform_msg.header.stamp = this->imu_stamp_;
+                  this->transform_msg.transform.translation.x = front_icp_srv_.response.new_laser_pose.x;
+                  this->transform_msg.transform.translation.y = front_icp_srv_.response.new_laser_pose.y;
+                  this->transform_msg.transform.translation.z = 0.0;
+                  tf2::Quaternion q;
+                  q.setRPY(0.0, 0.0, front_icp_srv_.response.new_laser_pose.theta);
+                  this->transform_msg.transform.rotation = tf2::toMsg(q);
+                  // this->transform_msg.transform.rotation.x = q.getRotation().x();
+                  // this->transform_msg.transform.rotation.y = q.getRotation().y();
+                  // this->transform_msg.transform.rotation.z = q.getRotation().z();
+                  // this->transform_msg.transform.rotation.w = q.getRotation().w();
+                  this->tf2_broadcaster.sendTransform(this->transform_msg);
+
                   this->front_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, front_blink_pose, front_cov, true);
                 }
                 else
@@ -163,7 +181,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                 this->debug_front_icp_last_scan_publisher_.publish(this->debug_front_icp_last_scan_msg_);
                 this->debug_front_icp_ref_scan_publisher_.publish(this->debug_front_icp_ref_scan_msg_);
                 if (this->front_icp_succedded_)
+                {
+                  this->debug_front_after_icp_scan_msg_.header.frame_id = "new_front_laser";
+                  this->debug_front_after_icp_scan_publisher_.publish(this->debug_front_after_icp_scan_msg_);
+
                   this->debug_front_icp_pose_publisher_.publish(this->debug_front_icp_pose_msg_);
+                }
               }
             }
             if (this->is_rear_icp_input_data_ok_ && set_last_ranges(false))
@@ -185,6 +208,22 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                                   rear_icp_srv_.response.covariance[3], rear_icp_srv_.response.covariance[4], rear_icp_srv_.response.covariance[5],
                                   rear_icp_srv_.response.covariance[6], rear_icp_srv_.response.covariance[7], rear_icp_srv_.response.covariance[8];
                   this->rear_icp_succedded_ = true;
+
+                  this->transform_msg.header.frame_id = this->rear_laser_frame_;
+                  this->transform_msg.child_frame_id = "new_rear_laser";
+                  this->transform_msg.header.stamp = this->imu_stamp_;
+                  this->transform_msg.transform.translation.x = rear_icp_srv_.response.new_laser_pose.x;
+                  this->transform_msg.transform.translation.y = rear_icp_srv_.response.new_laser_pose.y;
+                  this->transform_msg.transform.translation.z = 0.0;
+                  tf2::Quaternion q;
+                  q.setRPY(0.0, 0.0, rear_icp_srv_.response.new_laser_pose.theta);
+                  this->transform_msg.transform.rotation = tf2::toMsg(q);
+                  // this->transform_msg.transform.rotation.x = q.getRotation().x();
+                  // this->transform_msg.transform.rotation.y = q.getRotation().y();
+                  // this->transform_msg.transform.rotation.z = q.getRotation().z();
+                  // this->transform_msg.transform.rotation.w = q.getRotation().w();
+                  this->tf2_broadcaster.sendTransform(this->transform_msg);
+
                   this->rear_icp_succedded_ = get_base_link_new_pose_from_icp(new_laser_pose, covariances, rear_blink_pose, rear_cov, false);
                 }
                 else
@@ -200,7 +239,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                 this->debug_rear_icp_last_scan_publisher_.publish(this->debug_rear_icp_last_scan_msg_);
                 this->debug_rear_icp_ref_scan_publisher_.publish(this->debug_rear_icp_ref_scan_msg_);
                 if (this->rear_icp_succedded_)
+                {
+                  this->debug_rear_after_icp_scan_msg_.header.frame_id = "new_rear_laser";
+                  this->debug_rear_after_icp_scan_publisher_.publish(this->debug_rear_after_icp_scan_msg_);
+
                   this->debug_rear_icp_pose_publisher_.publish(this->debug_rear_icp_pose_msg_);
+                }
               }
             }
             // fuse poses?? call service to modify SLAM graph
@@ -261,6 +305,12 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   }
 
   // [fill msg structures]
+  // Initialize the topic message structure
+  //this->debug_rear_after_icp_scan_LaserScan_msg_.data = my_var;
+
+  // Initialize the topic message structure
+  //this->debug_front_after_icp_scan_LaserScan_msg_.data = my_var;
+
   // Initialize the topic message structure
   //this->debug_rear_icp_pose_PoseWithCovariance_msg_.data = my_var;
 
@@ -342,6 +392,30 @@ void CollisionManagerAlgNode::mainNodeThread(void)
   // [fill action structure and make request to the action server]
 
   // [publish messages]
+  // Uncomment the following line to publish the topic message
+  //this->debug_rear_after_icp_scan_publisher_.publish(this->debug_rear_after_icp_scan_LaserScan_msg_);
+
+  // Uncomment the following line to publish the topic message
+  //this->debug_front_after_icp_scan_publisher_.publish(this->debug_front_after_icp_scan_LaserScan_msg_);
+
+  
+  /*
+  //tf2_broadcaster example BEGIN
+  this->transform_msg.header.stamp    = ros::Time::now();
+  this->transform_msg.header.frame_id = "parent_frame";
+  this->transform_msg.child_frame_id  = "child_frame";
+  geometry_msgs::Transform t;
+  t.translation.x = 0.0;
+  t.translation.y = 0.0;
+  t.translation.z = 0.0;
+  tf2::Quaternion q;
+  q.setRPY(0.0, 0.0, 0.0);
+  t.rotation = tf2::toMsg(q);
+  this->transform_msg.transform = t;
+  this->tf2_broadcaster.sendTransform(this->transform_msg);
+  //tf2_broadcaster example END
+  */
+
   // Uncomment the following line to publish the topic message
   //this->debug_rear_icp_pose_publisher_.publish(this->debug_rear_icp_pose_PoseWithCovariance_msg_);
 
@@ -480,6 +554,8 @@ bool CollisionManagerAlgNode::set_last_ranges(bool _front)
     {
       (_front? this->debug_front_icp_last_scan_msg_: this->debug_rear_icp_last_scan_msg_).ranges = (_front? this->front_ranges_: this->rear_ranges_).front().ranges;
       (_front? this->debug_front_icp_last_scan_msg_: this->debug_rear_icp_last_scan_msg_).header.stamp = (_front? this->front_ranges_: this->rear_ranges_).front().stamp;
+      (_front? this->debug_front_after_icp_scan_msg_: this->debug_rear_after_icp_scan_msg_).ranges = (_front? this->front_ranges_: this->rear_ranges_).front().ranges;
+      (_front? this->debug_front_after_icp_scan_msg_: this->debug_rear_after_icp_scan_msg_).header.stamp = (_front? this->front_ranges_: this->rear_ranges_).front().stamp;
     }
     return true;
   }
@@ -611,6 +687,12 @@ void CollisionManagerAlgNode::rear_laser_scan_callback(const sensor_msgs::LaserS
     debug_rear_icp_last_scan_msg_.range_min = msg->range_min;
     debug_rear_icp_last_scan_msg_.range_max = msg->range_max;
     debug_rear_icp_last_scan_msg_.header.frame_id = msg->header.frame_id;
+    debug_rear_after_icp_scan_msg_.angle_min = msg->angle_min;
+    debug_rear_after_icp_scan_msg_.angle_max = msg->angle_max;
+    debug_rear_after_icp_scan_msg_.angle_increment = msg->angle_increment;
+    debug_rear_after_icp_scan_msg_.scan_time = msg->scan_time;
+    debug_rear_after_icp_scan_msg_.range_min = msg->range_min;
+    debug_rear_after_icp_scan_msg_.range_max = msg->range_max;
 
     RangesWithStamp r;
     r.ranges = msg->ranges;
@@ -665,6 +747,12 @@ void CollisionManagerAlgNode::front_laser_scan_callback(const sensor_msgs::Laser
     debug_front_icp_last_scan_msg_.range_min = msg->range_min;
     debug_front_icp_last_scan_msg_.range_max = msg->range_max;
     debug_front_icp_last_scan_msg_.header.frame_id = msg->header.frame_id;
+    debug_front_after_icp_scan_msg_.angle_min = msg->angle_min;
+    debug_front_after_icp_scan_msg_.angle_max = msg->angle_max;
+    debug_front_after_icp_scan_msg_.angle_increment = msg->angle_increment;
+    debug_front_after_icp_scan_msg_.scan_time = msg->scan_time;
+    debug_front_after_icp_scan_msg_.range_min = msg->range_min;
+    debug_front_after_icp_scan_msg_.range_max = msg->range_max;
 
     RangesWithStamp r;
     r.ranges = msg->ranges;