<launch>
  <include file="$(find openni_launch)/launch/openni.launch"/>
<node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />

  <!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
  <node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid pcl_manager" output="screen">
    <remap from="~input" to="/camera/depth_registered/points" />
    <rosparam>
      filter_field_name: z
      filter_limit_min: 0.01
      filter_limit_max: 1.5
      filter_limit_negative: False
      leaf_size: 0.01
    </rosparam>
  </node>

  <!-- Estimate point normals -->
  <node pkg="nodelet" type="nodelet" name="normal_estimation" args="load pcl/NormalEstimation pcl_manager" output="screen">
    <remap from="~input" to="/voxel_grid/output" />
    <rosparam>
      # -[ Mandatory parameters
      k_search: 0
      radius_search: 0.015
      # Set the spatial locator. Possible values are: 0 (ANN), 1 (FLANN), 2 (organized)
      spatial_locator: 0
    </rosparam>
  </node>

  <!-- Segment the table plane -->
  <node pkg="nodelet" type="nodelet" name="planar_segmentation" args="load pcl/SACSegmentationFromNormals pcl_manager" output="screen">
    <remap from="~input"   to="/voxel_grid/output" />
    <remap from="~normals" to="/normal_estimation/output" />
    <rosparam>
      # -[ Mandatory parameters
      model_type: 11
      distance_threshold: 0.1
      max_iterations: 1000
      method_type: 0
      optimize_coefficients: true
      normal_distance_weight: 0.1
      eps_angle: 0.09
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="extract_plane_indices" args="load pcl/ExtractIndices pcl_manager" output="screen">
    <remap from="~input"   to="/voxel_grid/output" />
    <remap from="~indices" to="/planar_segmentation/inliers" />
    <rosparam>
      negative: true
    </rosparam>
  </node>

  <!-- Project the planar inliers -->
  <node pkg="nodelet" type="nodelet" name="project_plane_inliers" args="load pcl/ProjectInliers pcl_manager" output="screen">
   <remap from="~input"   to="/voxel_grid/output" />
   <remap from="~indices" to="/planar_segmentation/inliers" />
   <remap from="~model"   to="/planar_segmentation/model" />
   <rosparam>
     model_type: 11
     copy_all_data: false
     copy_all_fields: true
   </rosparam>
  </node>

  <!-- Compute the convex hull -->
  <node pkg="nodelet" type="nodelet" name="convex_hull" args="load pcl/ConvexHull2D pcl_manager" output="screen">
    <remap from="~input"   to="/project_plane_inliers/output" />
  </node>

  <!-- Extract the object clusters using a polygonal prism -->
  <node pkg="nodelet" type="nodelet" name="extract_objects_table" args="load pcl/ExtractPolygonalPrismData pcl_manager" output="screen">
   <remap from="~input"         to="/extract_plane_indices/output" />
   <remap from="~planar_hull"   to="/convex_hull/output" />
   <rosparam>
     height_min: 0
     height_max: 0.5
   </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="extract_objects_indices" args="load pcl/ExtractIndices pcl_manager" output="screen">
    <!-- Extract_plane_indices needs to be negated for this work -->
    <remap from="~input"   to="/extract_plane_indices/output" />
    <remap from="~indices" to="/extract_objects_table/output" />
    <rosparam>
      negative: false
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="RobotVision" args="load robot_vision/RobotVision camera_nodelet_manager" output="screen">
    <remap from="/RobotVision/input" to="/camera/depth_registered/points"/>
  </node>
</launch>