<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" />

  <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: camera
      polygons:
      - points:
        - [1.5, -1.0, 2.0]
        - [-1.0, -1.0, 4.0]
        - [0.5,  1.0, 3.0]
        label: 1
        likelihood: 0.8
    </rosparam>
  </node>

  <node name="polygon_array_to_polygon"
        pkg="jsk_recognition_utils" type="polygon_array_to_polygon.py">
    <remap from="~input" to="polygon_array_publisher/output"/>
    <rosparam>
      index: 0
    </rosparam>
  </node>

  <node name="camera_info_publisher"
        pkg="rostopic" type="rostopic"
        args="pub -r 30 -s /camera/rgb/camera_info sensor_msgs/CameraInfo
              '{header: {stamp: now, frame_id: camera},
                height: 1080,
                width: 1920,
                K: [1064.3187262543424, 0.0, 960.0, 0.0, 1064.3981464626195, 540.0, 0.0, 0.0, 1.0],
                R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
                P: [1064.3187262543424, 0.0, 960.0, 0.0, 0.0, 1064.3981464626195, 540.0, 0.0, 0.0, 0.0, 1.0, 0.0]}'"/>

  <node name="polygon_to_mask_image"
        pkg="nodelet" type="nodelet"
        args="load jsk_perception/PolygonToMaskImage $(arg MANAGER)"
        respawn="true">
    <remap from="~input" to="polygon_array_to_polygon/output"/>
    <remap from="~input/camera_info" to="camera/rgb/camera_info"/>
  </node>

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