diff --git a/launch/collisions.launch b/launch/collisions.launch
index 3b15a7a75abbe624b378c36fa5941500707f2a27..928e93b9edcacac09359e802e16fe76b8f1cc63b 100644
--- a/launch/collisions.launch
+++ b/launch/collisions.launch
@@ -16,7 +16,7 @@
   <arg name="bias_filter_config_file" default="$(find iri_imu_bias_filter)/config/params.yaml"/>
   <arg name="imu_raw_topic"          default="/bno055_imu/imu"/>
   <arg name="imu_filtered_topic"     default="/bno055_imu/imu_bias_filtered"/>
-  <arg name="cmd_vel_topic"          default="/cmd_vel"/>
+  <arg name="odom_topic"              default="/odom"/>
   <arg name="update_bias_ns"         default="~/update_bias"/>
 
   <!--  Laser Scan ICP parameters  -->
@@ -27,7 +27,6 @@
   <arg name="collision_config_file"   default="$(find iri_collision_manager)/config/params.yaml"/>
   <arg name="front_laser_topic"       default="/laser_front/scan"/>
   <arg name="rear_laser_topic"        default="/laser_rear/scan"/>
-  <arg name="odom_topic"              default="/odom"/>
 
   <include file="$(find iri_bno055_imu_bringup)/launch/bno055_imu.launch"
            if="$(arg imu_driver)">
@@ -45,7 +44,7 @@
     <arg name="config_file" value="$(arg bias_filter_config_file)"/>
     <arg name="imu_topic_in" value="$(arg imu_raw_topic)"/>
     <arg name="imu_topic_out" value="$(arg imu_filtered_topic)"/>
-    <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
+    <arg name="odom_topic" value="$(arg odom_topic)"/>
     <arg name="update_bias_ns" value="$(arg update_bias_ns)"/>
   </include>
 
diff --git a/rviz/dogs.rviz b/rviz/dogs.rviz
index 0ecfb3722f56c1cc0f8c0f17cfd74850652354d6..62dc34192dccb7a70964b447b27ea6aeb9d7e129 100644
--- a/rviz/dogs.rviz
+++ b/rviz/dogs.rviz
@@ -54,10 +54,6 @@ Visualization Manager:
           Value: true
         map:
           Value: true
-        new_front_laser:
-          Value: true
-        new_rear_laser:
-          Value: true
         odom:
           Value: true
       Marker Scale: 1
@@ -70,11 +66,9 @@ Visualization Manager:
           odom:
             base_link:
               base_laser_front_link:
-                new_front_laser:
-                  {}
+                {}
               base_laser_rear_link:
-                new_rear_laser:
-                  {}
+                {}
               imu_bno055:
                 {}
       Update Interval: 0
@@ -119,7 +113,7 @@ Visualization Manager:
       Color: 239; 41; 41
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
@@ -134,7 +128,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -147,7 +141,7 @@ Visualization Manager:
       Color: 252; 233; 79
       Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
@@ -162,7 +156,7 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 6c6c6569ad91a99547212a14e692948dd98853ff..5296f3a591a71e71c352ce1242c0f5fa8757a5c2 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -171,20 +171,23 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                                   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);
+                  if (this->config_.debug)
+                  {
+                    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);
 
@@ -241,20 +244,23 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                                   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);
+                  if (this->config_.debug)
+                  {
+                    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);