diff --git a/launch/node.launch b/launch/node.launch
index 85165f9dbca67ca3e55f89b5f5f5318e6ac13e4a..b4dbb4f2d5b47d7430e24f8969814bd3a006c5aa 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -8,6 +8,9 @@
   <arg name="output" default="screen"/>
   <arg name="launch_prefix" default=""/>
   <arg name="cloud_in" default="~pointcloud_in"/>
+  <arg name="debug_image" default="~debug_image"/>
+  <arg name="obstacles_cloud_out" default="~obstacles_cloud_out"/>
+  <arg name="free_space_cloud_out" default="~free_space_cloud_out"/>
 
   <group ns="$(arg ns)">
 
@@ -17,6 +20,9 @@
           output="$(arg output)"
           launch-prefix="$(arg launch_prefix)">
       <remap from="~pointcloud_in" to="$(arg cloud_in)"/>
+      <remap from="~obstacles_img/image_raw" to="$(arg debug_image)"/>
+      <remap from="~obstacles" to="$(arg obstacles_cloud_out)"/>
+      <remap from="~free_space" to="$(arg free_space_cloud_out)"/>
       <rosparam file="$(arg config_file)" command="load"/>
       <rosparam file="$(arg ranges_file)" command="load"/>
     </node>
diff --git a/launch/nodelet.launch b/launch/nodelet.launch
index 260193ed8076386b42b2a851806d988aad37fc20..98d7dfab08ba120a4b3f845a2e78d2da4b271437 100644
--- a/launch/nodelet.launch
+++ b/launch/nodelet.launch
@@ -9,6 +9,9 @@
   <arg name="output" default="screen"/>
   <arg name="launch_prefix" default=""/>
   <arg name="cloud_in" default="~pointcloud_in"/>
+  <arg name="debug_image" default="~debug_image"/>
+  <arg name="obstacles_cloud_out" default="~obstacles_cloud_out"/>
+  <arg name="free_space_cloud_out" default="~free_space_cloud_out"/>
 
   <group ns="$(arg ns)">
 
@@ -18,6 +21,9 @@
           args="load iri_lidar_obstacle_detector/LidarObstacleDetectorAlgNodelet $(arg lidar_nodelet_manager)"
           output="$(arg output)">
       <remap from="~pointcloud_in" to="$(arg cloud_in)"/>
+      <remap from="~obstacles_img/image_raw" to="$(arg debug_image)"/>
+      <remap from="~obstacles" to="$(arg obstacles_cloud_out)"/>
+      <remap from="~free_space" to="$(arg free_space_cloud_out)"/>
       <rosparam file="$(arg config_file)" command="load"/>
       <rosparam file="$(arg ranges_file)" command="load"/>
     </node>