<launch>
  <arg name="gui" default="true"/>
  <arg name="launch_manager" default="true" />

  <arg name="MANAGER" default="sample_manager" />
  <node name="$(arg MANAGER)"
        pkg="nodelet" type="nodelet" args="manager"
        if="$(arg launch_manager)"
        output="screen" />

  <include file="$(find jsk_perception)/sample/sample_image_publisher.launch">
    <arg name="gui" value="false"/>
  </include>

  <node name="static_tf_publisher_map_to_camera"
        pkg="tf" type="static_transform_publisher"
        args="-0.580 -1.485 1.289 -0.753 0.291 -0.217 0.548 /map /camera 10"/>

  <node name="polygon_array_publisher"
        pkg="jsk_recognition_utils" type="polygon_array_publisher.py"
        output="screen">
    <rosparam>
      frame_id: map
      publish_rate: 30.0
      polygons:
      - points:
        - [1.0, -1.0, 0.0]
        - [2.0, -1.0, 0.0]
        - [2.0,  1.0, 0.0]
        - [1.0,  1.0, 0.0]
        label: 1
        likelihood: 0.8
      - points:
        - [1.0, -1.0, 0.5]
        - [2.0, -1.0, 0.5]
        - [2.0,  1.0, 0.5]
        - [1.0,  1.0, 0.5]
        label: 2
        likelihood: 0.6
      - points:
        - [1.0, -1.0, 1.0]
        - [2.0, -1.0, 1.0]
        - [2.0,  1.0, 1.0]
        - [1.0,  1.0, 1.0]
        label: 3
        likelihood: 0.4
      - points:
        - [0.0,  1.0, 0.0]
        - [0.8,  1.0, 0.0]
        - [0.8,  1.0, 1.0]
        - [0.0,  1.0, 1.0]
        label: 4
        likelihood: 0.8
    </rosparam>
  </node>

  <node name="tabletop_color_difference_likelihood"
        pkg="nodelet" type="nodelet"
        args="load jsk_perception/TabletopColorDifferenceLikelihood $(arg MANAGER)"
        respawn="true" >
    <remap from="~input" to="raw_image_rgb/image_color"/>
    <remap from="~input/polygons" to="polygon_array_publisher/output"/>
    <remap from="~input/camera_info" to="raw_image_rgb/camera_info"/>
    <rosparam>
      tf_queue_size: 100
      histogram_top_n_ratio: 0.25
    </rosparam>
  </node>

  <group if="$(arg gui)">
    <node name="rviz"
          pkg="rviz" type="rviz"
          args="-d $(find jsk_perception)/sample/config/sample_tabletop_color_difference_likelihood.rviz"/>
  </group>
</launch>
