<launch>
  <arg name="image_left" default="/kinfu/output/rendered_image" />
  <arg name="image_right" default="/kinfu/output/rendered_image" />

  <arg name="CAMERA_INFO" default="/kinect_head/depth_registered/camera_info" />
  <arg name="IMAGE_DEPTH" default="/kinect_head/depth_registered/image_rect" />
  <arg name="IMAGE_COLOR" default="/kinect_head/rgb/image_rect_color" />
  <arg name="DECOMP_CAMERA_INFO" default="/kinect_head_decompressed/depth_registered/camera_info" />
  <arg name="DECOMP_RGB_CAMERA_INFO" default="/kinect_head_decompressed/rgb/camera_info" />
  <arg name="DECOMP_IMAGE_DEPTH" default="/kinect_head_decompressed/depth_registered/image_rect" />
  <arg name="DECOMP_IMAGE_COLOR" default="/kinect_head_decompressed/rgb/image_rect_color" />
  <arg name="DECOMP_POINTS" default="/kinect_head_decompressed/depth_registered/points" />

  <group ns="kinect_head_decompressed">
    <node name="camera_info_relay" pkg="topic_tools" type="relay"
          args="$(arg CAMERA_INFO) $(arg DECOMP_CAMERA_INFO)" />
    <node name="rgb_camera_info_relay" pkg="topic_tools" type="relay"
          args="$(arg CAMERA_INFO) $(arg DECOMP_RGB_CAMERA_INFO)" />
    <node name="depth_image_transport" pkg="image_transport" type="republish"
          args="compressedDepth in:=$(arg IMAGE_DEPTH) raw out:=$(arg DECOMP_IMAGE_DEPTH)" />
    <node name="rgb_image_transport" pkg="image_transport" type="republish"
          args="compressed in:=$(arg IMAGE_COLOR) raw out:=$(arg DECOMP_IMAGE_COLOR)" />
    <node name="point_cloud_xyzrgb" pkg="nodelet" type="nodelet"
          args="standalone depth_image_proc/point_cloud_xyzrgb" output="screen" />
  </group>

  <node name="kinfu"
        pkg="nodelet" type="nodelet"
        args="standalone jsk_pcl/Kinfu"
        output="screen">
    <remap from="~input/camera_info" to="$(arg DECOMP_CAMERA_INFO)" />
    <remap from="~input/depth" to="$(arg DECOMP_IMAGE_DEPTH)" />
    <remap from="~input/color" to="$(arg DECOMP_IMAGE_COLOR)" />
    <rosparam subst_value="true">
      auto_reset: true
      integrate_color: true
      save_dir: $(optenv HOME)/.ros/kinfu
      n_textures: -1
    </rosparam>
  </node>
</launch>