<!--
Copyright (c) 2018 Intel Corporation

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->

<!--
A launch file, derived from rgbd_launch and customized for Realsense ROS driver,
to publish XYZRGB point cloud like an OpenNI camera.

NOTICE: To use this launch file you must first install ros package rgbd_launch.

To launch Realsense with software registeration (ROS Image Pipeline and rgbd_launch):
    $ roslaunch realsense2_camera rs_rgbd.launch
Processing enabled by ROS driver:
    # depth rectification
Processing enabled by this node:
    # rgb rectification
    # depth registeration
    # pointcloud_xyzrgb generation

To launch Realsense with hardware registeration (ROS Realsense depth alignment):
    $ roslaunch realsense2_camera rs_rgbd.launch align_depth:=true
Processing enabled by ROS driver:
    # depth rectification
    # depth registration
Processing enabled by this node:
    # rgb rectification
    # pointcloud_xyzrgb generation
-->

<launch>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="external_manager"    default="false"/>
  <arg name="manager"             default="realsense2_camera_manager"/>

  <!-- Camera device specific arguments -->

  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default=""/>
  <arg name="json_file_path"      default=""/>

  <arg name="fisheye_width"       default="-1"/>
  <arg name="fisheye_height"      default="-1"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="-1"/>
  <arg name="depth_height"        default="-1"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="infra_width"         default="-1"/>
  <arg name="infra_height"        default="-1"/>
  <arg name="enable_infra1"       default="false"/>
  <arg name="enable_infra2"       default="false"/>

  <arg name="color_width"         default="-1"/>
  <arg name="color_height"        default="-1"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="-1"/>
  <arg name="depth_fps"           default="-1"/>
  <arg name="infra_fps"           default="-1"/>
  <arg name="color_fps"           default="-1"/>
  <arg name="gyro_fps"            default="-1"/>
  <arg name="accel_fps"           default="-1"/>
  <arg name="enable_gyro"         default="false"/>
  <arg name="enable_accel"        default="false"/>

  <arg name="enable_pointcloud"   default="false"/>
  <arg name="enable_sync"         default="true"/>
  <arg name="align_depth"         default="true"/>
  <arg name="filters"             default=""/>

  <arg name="publish_tf"          default="true"/>
  <arg name="tf_publish_rate"     default="0"/> <!-- 0 - static transform -->

  <!-- rgbd_launch specific arguments -->

  <!-- Arguments for remapping all device namespaces -->
  <arg name="rgb"                             default="color" />
  <arg name="ir"                              default="infra1" />
  <arg name="depth"                           default="depth" />
  <arg name="depth_registered_pub"            default="depth_registered" />
  <arg name="depth_registered"                default="depth_registered" unless="$(arg align_depth)" />
  <arg name="depth_registered"                default="aligned_depth_to_color" if="$(arg align_depth)" />
  <arg name="depth_registered_filtered"       default="$(arg depth_registered)" />
  <arg name="projector"                       default="projector" />

  <!-- Disable bond topics by default -->
  <arg name="bond"                            default="false" />
  <arg name="respawn"                         default="$(arg bond)" />

  <!-- Processing Modules -->
  <arg name="rgb_processing"                  default="true"/>
  <arg name="debayer_processing"              default="false" />
  <arg name="ir_processing"                   default="false"/>
  <arg name="depth_processing"                default="false"/>
  <arg name="depth_registered_processing"     default="true"/>
  <arg name="disparity_processing"            default="false"/>
  <arg name="disparity_registered_processing" default="false"/>
  <arg name="hw_registered_processing"        default="$(arg align_depth)" />
  <arg name="sw_registered_processing"        default="true" unless="$(arg align_depth)" />
  <arg name="sw_registered_processing"        default="false" if="$(arg align_depth)" />

  <group ns="$(arg camera)">

    <!-- Launch the camera device nodelet-->
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="external_manager"         value="$(arg external_manager)"/>
      <arg name="manager"                  value="$(arg manager)"/>
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>
      <arg name="filters"                  value="$(arg filters)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>
    </include>

    <!-- RGB processing -->
    <include if="$(arg rgb_processing)"
             file="$(find rgbd_launch)/launch/includes/rgb.launch.xml">
      <arg name="manager"                       value="$(arg manager)" />
      <arg name="respawn"                       value="$(arg respawn)" />
      <arg name="rgb"                           value="$(arg rgb)" />
      <arg name="debayer_processing"            value="$(arg debayer_processing)" />
    </include>

    <group if="$(eval depth_registered_processing and sw_registered_processing)">
      <node pkg="nodelet" type="nodelet" name="register_depth"
            args="load depth_image_proc/register $(arg manager) $(arg bond)" respawn="$(arg respawn)">
        <remap from="rgb/camera_info"             to="$(arg rgb)/camera_info" />
        <remap from="depth/camera_info"           to="$(arg depth)/camera_info" />
        <remap from="depth/image_rect"            to="$(arg depth)/image_rect_raw" />
        <remap from="depth_registered/image_rect" to="$(arg depth_registered)/sw_registered/image_rect_raw" />
      </node>

      <!-- Publish registered XYZRGB point cloud with software registered input -->
      <node pkg="nodelet" type="nodelet" name="points_xyzrgb_sw_registered"
            args="load depth_image_proc/point_cloud_xyzrgb $(arg manager) $(arg bond)" respawn="$(arg respawn)">
        <remap from="rgb/image_rect_color"        to="$(arg rgb)/image_rect_color" />
        <remap from="rgb/camera_info"             to="$(arg rgb)/camera_info" />
        <remap from="depth_registered/image_rect" to="$(arg depth_registered_filtered)/sw_registered/image_rect_raw" />
        <remap from="depth_registered/points"     to="$(arg depth_registered)/points" />
      </node>
    </group>

    <group if="$(eval depth_registered_processing and hw_registered_processing)">
      <!-- Publish registered XYZRGB point cloud with hardware registered input (ROS Realsense depth alignment) -->
      <node pkg="nodelet" type="nodelet" name="points_xyzrgb_hw_registered"
            args="load depth_image_proc/point_cloud_xyzrgb $(arg manager) $(arg bond)" respawn="$(arg respawn)">
        <remap from="rgb/image_rect_color"        to="$(arg rgb)/image_rect_color" />
        <remap from="rgb/camera_info"             to="$(arg rgb)/camera_info" />
        <remap from="depth_registered/image_rect" to="$(arg depth_registered)/image_raw" />
        <remap from="depth_registered/points"     to="$(arg depth_registered_pub)/points" />
      </node>
    </group>

  </group>

</launch>
