<launch>
  <remap from="/diagnostics" to="/diagnostics_pcl" />
  <remap from="/diagnostics_agg" to="/diagnostics_pcl_agg" />
  <!-- <arg name="BASE_FRAME_ID" default="odom_combined" /> -->
  <arg name="BASE_FRAME_ID" default="base_footprint" />
  <arg name="RUN_SELF_FILTER" default="true"/>
  <arg name="SELF_FILTER_PARAM" default="$(find jsk_pr2_startup)/jsk_pr2_sensors/tilt_self_filter.yaml" />
  <arg name="JOINT_STATIC_FILTER" default="true" />
  <arg name="RUN_NONHORIZONTAL_OCTOMAP_SERVER" default="false"/>
  <arg name="FILTER_JOINT_PARAM" value="$(find jsk_pcl_ros)/config/pr2_sensor_joint.yaml" />
  <arg name="INPUT" default="/openni_c2/depth_registered_half/points" />
  <arg name="ICP_REGISTRATION" default="false" />
  <arg name="MODEL_FILE" default="$(find jsk_pcl_ros)/models/hand_drill_kinect.pcd" />
  <arg name="LAUNCH_MANAGER" default="true" />
  <arg name="MANAGER" default="tabletop_manager" />
  <arg name="ESTIMATE_OCCLUSION" default="false" />
  <arg name="COLOR_HISTOGRAM" default="false" />
  <arg name="HANDLE_ESTIMATOR" default="true" />
  <arg name="MACHINE" default="localhost"/>
  <arg name="GDB" default="false" />
  <arg name="ENV_SERVER" default="true" />
  <arg name="SAVE_SELECTED_CLOUD" default="false" />
  <arg name="INTERACTIVE_POINT_CLOUD" default="true" />
  <arg name="GRASPING_CONTROLLER" default="true" />
  <machine name="localhost" address="localhost" />
  <group if="$(arg LAUNCH_MANAGER)">
    <node pkg="nodelet" type="nodelet" name="$(arg MANAGER)"
          args="manager"
          machine="$(arg MACHINE)"
          if="$(arg GDB)"
          launch-prefix="xterm -e gdb --args"
          output="screen"/>
    <node pkg="nodelet" type="nodelet" name="$(arg MANAGER)"
          args="manager"
          unless="$(arg GDB)"
          machine="$(arg MACHINE)"
          output="screen"/>
  </group>
<group if="true">
  <group if="$(arg RUN_SELF_FILTER)">
    <node pkg="pr2_navigation_self_filter"
          type="self_filter" clear_params="true"
          name="openni_cloud_self_filter" respawn="true" output="screen">
      <remap from="cloud_in" to="$(arg INPUT)" />
      <remap from="cloud_out" to="~cloud_out" />
      <rosparam command="load" file="$(arg SELF_FILTER_PARAM)" />
      <param name="use_rgb" value="true" />
      <param name="keep_organized" value="true"/>
      <param name="subsample_value" type="double" value="0.0"/>
    </node>
  </group>

  <node pkg="nodelet" type="nodelet" name="input_relay"
        machine="$(arg MACHINE)"
        args="load jsk_topic_tools/Relay $(arg MANAGER)">
    <remap from="~input" to="$(arg INPUT)" unless="$(arg RUN_SELF_FILTER)" />
    <remap from="~input" to="openni_cloud_self_filter/cloud_out" if="$(arg RUN_SELF_FILTER)" />
    <remap from="~output" to="~raw_output" if="$(arg JOINT_STATIC_FILTER)" />
  </node>
  <!-- in preparation -->
  <group if="$(arg INTERACTIVE_POINT_CLOUD)">
    <include file="$(find jsk_interactive_marker)/launch/bounding_box_grasp.launch"/>
  </group>

  <group if="$(arg GRASPING_CONTROLLER)">
    <node pkg="roseus" type="roseus" name="grasp_ik_controller"
          args="$(find jsk_interactive_marker)/euslisp/ik-controller-main.l">
      <remap from="/robot/joint_state_decompressed" to="/joint_states"/>
      <param name="one_click_grasp" value="true"/>
    </node>
  </group>
</group>
  <node pkg="nodelet" type="nodelet" name="joint_static_filter"
        if="$(arg JOINT_STATIC_FILTER)"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/JointStateStaticFilter /$(arg MANAGER)">
    <remap from="~input_joint_state" to="/joint_states" />
    <remap from="~input" to="input_relay/raw_output"/>
    <remap from="~output" to="input_relay/output" />
    <rosparam command="load" file="$(arg FILTER_JOINT_PARAM)" />
  </node>
  <node pkg="nodelet" type="nodelet" name="multi_plane_estimate"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/OrganizedMultiPlaneSegmentation $(arg MANAGER)"
        clear_params="false">
    <remap from="~input" to="input_relay/output"/>
    <rosparam>
      max_curvature: 0.01
      estimate_normal: true
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="ground_wall_separator"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/PlaneReasoner $(arg MANAGER)"
        clear_params="false">
    <param name="global_frame_id" value="$(arg BASE_FRAME_ID)"/>
    <remap from="~input" to="input_relay/output"/>
    <remap from="~input_inliers" to="multi_plane_estimate/output_refined" />
    <remap from="~input_coefficients" to="multi_plane_estimate/output_refined_coefficients" />
    <remap from="~input_polygons" to="multi_plane_estimate/output_refined_polygon" />
  </node>

  <node pkg="nodelet" type="nodelet"
        name="vertical_plane_extraction"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/MultiPlaneExtraction $(arg MANAGER)"
        output="screen">
    <remap from="~input" to="input_relay/output" />
    <remap from="~indices" to="ground_wall_separator/output/vertical/inliers" />
    <remap from="~input_polygons" to="ground_wall_separator/output/vertical/polygons" />
    <remap from="~input_coefficients" to="ground_wall_separator/output/vertical/coefficients" />
  </node>
  
  <node pkg="nodelet" type="nodelet"
        name="horizontal_plane_extraction"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/MultiPlaneExtraction $(arg MANAGER)"
        output="screen">
    <remap from="~input" to="input_relay/output" />
    <remap from="~indices" to="ground_wall_separator/output/horizontal/inliers" />
    <remap from="~input_polygons" to="ground_wall_separator/output/horizontal/polygons" />
    <remap from="~input_coefficients" to="ground_wall_separator/output/horizontal/coefficients" />
  </node>

  <node pkg="nodelet" type="nodelet" name="horizontal_planes_respected_to_odom"
        args="load jsk_pcl/PolygonArrayTransformer /$(arg MANAGER)">
    <remap from="~input_polygons" to="ground_wall_separator/output/horizontal/polygons" />
    <remap from="~input_coefficients" to="ground_wall_separator/output/horizontal/coefficients" />
      <param name="frame_id" value="$(arg BASE_FRAME_ID)" />
  </node>
  
  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server"
        if="$(arg RUN_NONHORIZONTAL_OCTOMAP_SERVER)">
    <param name="resolution" value="0.05" />
    <param name="frame_id" type="string" value="$(arg BASE_FRAME_ID)" />
    <param name="sensor_model/max_range" value="5.0" />
    <remap from="cloud_in" to="horizontal_plane_extraction/output_nonplane_cloud" />
  </node>
  
  <node pkg="nodelet" type="nodelet" name="colorize_error"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/ColorizeDistanceFromPlane $(arg MANAGER)"
        clear_params="false">
    <remap from="~input" to="input_relay/output"/>
    <remap from="~input_indices" to="multi_plane_estimate/output_refined"/>
    <remap from="~input_coefficients" to="multi_plane_estimate/output_refined_coefficients"/>
    <remap from="~input_polygons" to="/multi_plane_estimate/output_refined_polygon" />
  </node>
  
  <node pkg="nodelet" type="nodelet"
        name="cluster_decomposer"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/ClusterPointIndicesDecomposer $(arg MANAGER)"
        output="screen">
    <remap from="~input" to="input_relay/output" />
    <remap from="~target" to="/multi_plane_estimate/output" />
    <rosparam>
      publish_clouds: false
      publish_tf: false
    </rosparam>
  </node>
  <node pkg="nodelet" type="nodelet"
        name="cluster_decomposer2"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/ClusterPointIndicesDecomposer $(arg MANAGER)"
        output="screen">
    <remap from="~input" to="input_relay/output" />
    <remap from="~target" to="/multi_plane_estimate/output_nonconnected" />
    <rosparam>
      publish_clouds: false
      publish_tf: false
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet"
        name="plane_extraction"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/MultiPlaneExtraction $(arg MANAGER)"
        output="screen">
    <remap from="~input" to="input_relay/output" />
    <remap from="~indices" to="/multi_plane_estimate/output_refined" />
    <remap from="~input_polygons" to="/multi_plane_estimate/output_refined_polygon" />
    <remap from="~input_coefficients" to="/multi_plane_estimate/output_refined_coefficients" />
  </node>
  
  <node pkg="nodelet" type="nodelet" name="euclidean_clustering"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/EuclideanClustering /$(arg MANAGER)" output="screen">
    <remap from="~input" to="/plane_extraction/output" />
    <rosparam>
      tolerance: 0.02
      min_size: 100
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet"
        name="cluster_decomposer_final"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/ClusterPointIndicesDecomposer /$(arg MANAGER)"
        output="screen">
    <remap from="~input" to="/plane_extraction/output" />
    <remap from="~target" to="/euclidean_clustering/output_throttle" />
    <remap from="~align_planes" to="/multi_plane_estimate/output_refined_polygon" />
    <remap from="~align_planes_coefficients"
           to="/multi_plane_estimate/output_refined_coefficients" />
    <rosparam>
      align_boxes: true
      use_pca: true
      publish_clouds: false
    </rosparam>
  </node>
  <node name="bounding_box_filter"
        pkg="nodelet" type="nodelet"
        if="false"
        args="load jsk_pcl/BoundingBoxFilter /$(arg MANAGER)">
    <remap from="~input_box" to="cluster_decomposer_final/boxes" />
    <remap from="~input_indices" to="euclidean_clustering/output_throttle" />
  </node>
  <node pkg="nodelet" type="nodelet"
        name="throttle_segmentation"
        args="load jsk_topic_tools/LightweightThrottle /$(arg MANAGER)"
        output="screen">
    <remap from="~input" to="euclidean_clustering/output" />
    <remap from="~output" to="euclidean_clustering/output_throttle" />
  </node>
  <node pkg="jsk_interactive_marker"
        type="bounding_box_marker"
        machine="$(arg MACHINE)"
        name="bounding_box_marker"
        output="screen"
        >
    <remap from="~bounding_box_array" to="cluster_decomposer_final/boxes" />
  </node>
  <node pkg="nodelet" type="nodelet"
        name="selected_cloud"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/SelectedClusterPublisher /$(arg MANAGER)"
        output="screen">
    <remap from="~input" to="/plane_extraction/output" />
    <remap from="~indices" to="/euclidean_clustering/output" />
    <remap from="~selected_index" to="/bounding_box_marker/selected_index" />
    <remap from="~output" to="/selected_pointcloud" />
  </node>
  <group if="$(arg COLOR_HISTOGRAM)">
    <node pkg="nodelet" type="nodelet" name="color_histogram"
          args="load jsk_pcl/ColorHistogramMatcher /$(arg MANAGER)"
          machine="$(arg MACHINE)"
          output="screen">
      <!-- <remap from="~input" to="/plane_extraction/output" />-->
      <remap from="~input" to="input_relay/output"/>
      <remap from="~input_indices" to="grid_sampler/output" />
      <!-- <remap from="~input_indices" to="euclidean_clustering/output" /> -->
      <remap from="~input_reference_cloud" to="/selected_pointcloud" />
      <rosparam>
        bin_size: 20
      </rosparam>
    </node>
    <node pkg="nodelet" type="nodelet" name="grid_sampler"
          args="load jsk_pcl/GridSampler /$(arg MANAGER)"
          machine="$(arg MACHINE)"
          output="screen">
      <!-- <remap from="~input" to="/plane_extraction/output" /> -->
      <remap from="~input" to="input_relay/output"/>
      <rosparam>
        grid_size: 0.1
      </rosparam>
    </node>
    <node pkg="nodelet" type="nodelet"
          name="grid_sample_decompose"
          machine="$(arg MACHINE)"
          args="load jsk_pcl/ClusterPointIndicesDecomposer /$(arg MANAGER)"
          output="screen">
      <!-- <remap from="~input" to="/plane_extraction/output" /> -->
      <remap from="~input" to="input_relay/output"/>
      <remap from="~target" to="grid_sampler/output" />
      <rosparam>
        publish_clouds: false
        publish_tf: false
      </rosparam>
    </node>
    <node pkg="nodelet" type="nodelet"
          name="match_result"
          machine="$(arg MACHINE)"
          args="load jsk_pcl/ClusterPointIndicesDecomposer /$(arg MANAGER)"
          output="screen">
      <!-- <remap from="~input" to="/plane_extraction/output" /> -->
      <remap from="~input" to="input_relay/output"/>
      <remap from="~target" to="color_histogram/output" />
      <rosparam>
        publish_clouds: false
      </rosparam>
    </node>
  </group>
  <node pkg="nodelet" type="nodelet" name="handle_estimator"
        if="$(arg HANDLE_ESTIMATOR)"
        machine="$(arg MACHINE)"
        args="load jsk_pcl/HandleEstimator /$(arg MANAGER)">
    <remap from="~input" to="input_relay/output" />
    <remap from="~input_box" to="/bounding_box_marker/selected_box" />
    <rosparam>
      approach_offset: 0.1
      gripper_size: 0.12
    </rosparam>
  </node>
  
  <group if="true">
    <!-- PR2 base footprint -->
    <node pkg="nodelet" type="nodelet" name="footstep_polygon_publisher"
          clear_params="true"
          args="load jsk_pcl/StaticPolygonArrayPublisher /$(arg MANAGER)">
      <remap from="~input" to="input_relay/output" />
      <rosparam>
        use_message: true
        frame_ids: [base_footprint]
        polygon_array: [[[-0.325,-0.325,0.0],[-0.425,-0.175,0.0],[-0.425,0.175,0.0],[-0.325,0.325,0.0],[0.325,0.325,0.0],[0.375,0,0.0],[0.325,-0.325,0.0]]]
      </rosparam>
    </node>
    <node pkg="nodelet" type="nodelet" name="footstep_respected_to_odom"
          args="load jsk_pcl/PolygonArrayTransformer /$(arg MANAGER)">
      <remap from="~input_polygons" to="footstep_polygon_publisher/output_polygons" />
      <remap from="~input_coefficients" to="footstep_polygon_publisher/output_coefficients" />
      <param name="frame_id" value="$(arg BASE_FRAME_ID)" />
    </node>
    <node pkg="nodelet" type="nodelet" name="transform_input"
          machine="$(arg MACHINE)"
          args="load jsk_pcl/TfTransformCloud $(arg MANAGER)">
      <remap from="~input" to="input_relay/output" />
      <param name="target_frame_id" value="$(arg BASE_FRAME_ID)" />
    </node>
    <node pkg="jsk_pcl_ros" type="environment_plane_modeling" name="env_server" output="screen"
          >
      <remap from="~input" to="transform_input/output" />
      <remap from="~indices" to="ground_wall_separator/output/horizontal/inliers" />
      <remap from="~input_polygons" to="horizontal_planes_respected_to_odom/output_polygons" />
      <remap from="~input_coefficients" to="horizontal_planes_respected_to_odom/output_coefficients" />
      <remap from="~input_static_polygons" to="/footstep_respected_to_odom/output_polygons" />
      <remap from="~input_static_coefficients" to="/footstep_respected_to_odom/output_coefficients" />
      <param name="continuous_estimation" value="true" />
      <param name="history_accumulation" value="true" />
    </node>
    <node pkg="nodelet" type="nodelet" name="estimated_plane_respected_to_odom"
          args="load jsk_pcl/PolygonArrayTransformer /$(arg MANAGER)">
      <remap from="~input_polygons" to="multi_plane_estimate/output_refined_polygon" />
      <remap from="~input_coefficients" to="multi_plane_estimate/output_refined_coefficients" />
      <param name="frame_id" value="$(arg BASE_FRAME_ID)" />
    </node>
  </group>

  <node pkg="diagnostic_aggregator" type="aggregator_node"
        respawn="true"
        name="pcl_diagnostic_aggregator">
  </node>
  <node pkg="rqt_robot_monitor" type="rqt_robot_monitor"
        name="pcl_diagnostics_monitor" />

  <group if="$(arg SAVE_SELECTED_CLOUD)">
    <node pkg="nodelet" type="nodelet" name="pointcloud_transform_in_box"
          args="load jsk_pcl/TransformPointcloudInBoundingBox /$(arg MANAGER)">
      <remap from="~input" to="/selected_pointcloud" />
      <remap from="~input_box" to="/bounding_box_marker/selected_box" />
    </node>
    <node pkg="nodelet" type="nodelet" name="pcd_saver"
          args="load pcl/PCDWriter /$(arg MANAGER)">
      <remap from="~input" to="pointcloud_transform_in_box/output" />
    </node>
  </group>
  <group if="$(arg ICP_REGISTRATION)">
    <node pkg="nodelet" type="nodelet" name="icp_registration"
          args="load jsk_pcl/ICPRegistration /$(arg MANAGER)">
      <param name="align_box" value="true" />
      <remap from="~input" to="/selected_pointcloud" />
      <remap from="~input_box" to="/bounding_box_marker/selected_box" />
      <remap from="~input_reference" to="reference_cloud" />
    </node>
    <node pkg="pcl_ros" type="pcd_to_pointcloud" name="model_publisher"
          args="$(arg MODEL_FILE)">
      <remap from="cloud_pcd" to="reference_cloud" />
    </node>
  </group>

  <node pkg="nodelet" type="nodelet" name="pointcloud_db"
        args="load jsk_pcl/PointcloudDatabaseServer /$(arg MANAGER)">
    <rosparam subst_value="true">
      models: ["$(find jsk_pcl_ros)/models/hand_drill_kinect.pcd", "$(find jsk_pcl_ros)/models/takenoko_drill_kinect.pcd"]
    </rosparam>
  </node>
</launch>
