<launch>
  <arg name="USE_NODELET_MANAGER"  default="true"/>
  <arg name="NODELET_MANAGER_NAME" default="heightmap_nodelet"/>
  <arg name="NODELET_INDEX"        default="0"/>

  <arg name="INPUT" default="/distance_filtered_laser_snapshots/output_cloud" />
  <arg name="TO_HEIGHTMAP_CONVERTER" value="/heightmap_converter_input" />
  <arg name="DEBUG_VIEW" default="true" />

  <arg name="MAXIMUM_Z" default="2.3" /> <!-- lower than ceiling -->
  <arg name="MINIMUM_Z" default="-2.0" />
  <!-- frames -->
  <arg name="STATIC_FRAME" default="map" />
  <arg name="STAND_FRAME"  default="odom_init" />
  <arg name="ROBOT_FRAME"  default="body_on_odom" />  <!--it's not good-->
  <arg name="PROJECTED_FRAME" default="BODY_on_map" />
  <arg name="USE_PROJECTED_FRAME" default="true" />

  <node pkg="jsk_footstep_controller" type="robot-boundingbox.l" name="robot_bounding_box">
    <rosparam>
      analysis_level: whole_links
      padding_x: 0.3
      padding_y: 0.3
      padding_z: -0.015
      height_offset: 1.5
    </rosparam>
  </node>

  <!-- height map -->
  <include file="$(find jsk_pcl_ros)/launch/heightmap_converter.launch">
    <arg name="USE_NODELET_MANAGER"  value="$(arg USE_NODELET_MANAGER)"/>
    <arg name="NODELET_MANAGER_NAME" value="$(arg NODELET_MANAGER_NAME)"/>
    <arg name="NODELET_INDEX"        value="1$(arg NODELET_INDEX)"/>
    <arg name="DEBUG_VIEW"           value="$(arg DEBUG_VIEW)" />

    <arg name="INPUT" value="$(arg TO_HEIGHTMAP_CONVERTER)" />

    <arg name="STATIC_FRAME" default="$(arg STATIC_FRAME)" />
    <arg name="ROBOT_FRAME"  default="$(arg ROBOT_FRAME)" />
    <arg name="PROJECTED_FRAME" default="$(arg PROJECTED_FRAME)" />
    <arg name="USE_PROJECTED_FRAME" default="$(arg USE_PROJECTED_FRAME)" />
  </include>

  <!-- nodelets -->
  <group ns="$(arg NODELET_MANAGER_NAME)">
    <rosparam subst_value="true">
      nodelets_$(arg NODELET_INDEX):
        - name: map_origin_polygon
          type: jsk_pcl/StaticPolygonArrayPublisher
          remappings:
            - from: ~input
              to: robot_center_pointcloud/output
        - name: map_origin_polygon_sampler
          type: jsk_pcl/PolygonPointsSampler
          remappings:
            - from: ~input/polygons
              to: map_origin_polygon/output_polygons
            - from: ~input/coefficients
              to: map_origin_polygon/output_coefficients
        - name: robot_center_map_origin_points
          type: jsk_pcl/TfTransformCloud
          remappings:
            - from: ~input
              to: map_origin_polygon_sampler/output_xyz
        - name: robot_center_pointcloud
          type: jsk_pcl/TfTransformCloud
          remappings:
            - from: ~input
              to: $(arg INPUT)
        - name: robot_bbox_clipper
          type: jsk_pcl/AttentionClipper
          remappings:
            - from: ~input/points
              to: /robot_center_pointcloud/output
            - from: ~input/box
              to: /robot_bbox
        - name: robot_center_pointcloud_bbox_clipped
          type: jsk_pcl/ExtractIndices
          remappings:
            - from: ~input
              to: /robot_center_pointcloud/output
            - from: ~indices
              to: /robot_bbox_clipper/output/point_indices
        - name: robot_center_pointcloud_with_map_origin_points
          type: pcl/PointCloudConcatenateDataSynchronizer
        - name: pass_through_z
          type: pcl/PassThrough
          remappings:
            - from: ~input
              to: robot_center_pointcloud_with_map_origin_points/output
            - from: ~output
              to: $(arg TO_HEIGHTMAP_CONVERTER)
    </rosparam>
  </group>

  <!-- parameters -->
  <rosparam param="map_origin_polygon" subst_value="true">
    use_message: true
    frame_ids: [$(arg STAND_FRAME)]
    # polygon_array: [[[0.5, 0.5, -0.01], [-0.5, 0.5, -0.01], [-0.5, -0.5, -0.01], [0.5, -0.5, -0.01]]]
    polygon_array: [[[0.25, 0.25, -0.01], [-0.25, 0.25, -0.01], [-0.25, -0.25, -0.01], [0.25, -0.25, -0.01]]]
  </rosparam>
  <rosparam param="robot_center_map_origin_points" subst_value="true">
    target_frame_id: $(arg ROBOT_FRAME)
  </rosparam>
  <rosparam param="robot_center_pointcloud" subst_value="true">
    target_frame_id: $(arg ROBOT_FRAME)
  </rosparam>
  <rosparam param="robot_center_pointcloud_with_map_origin_points" subst_value="true">
    input_topics: ["/robot_center_pointcloud_bbox_clipped/output", "/robot_center_map_origin_points/output"]
    output_frame: $(arg ROBOT_FRAME)
    max_queue_size: 50
  </rosparam>
  <rosparam param="robot_bbox_clipper" subst_value="true">
    use_multiple_attention: false
    frame_id: $(arg ROBOT_FRAME)
  </rosparam>
  <rosparam param="robot_center_pointcloud_bbox_clipped" subst_value="true">
    negative: true
  </rosparam>
  <rosparam param="pass_through_z" subst_value="true" >
    filter_field_name: z
    filter_limit_min: $(arg MINIMUM_Z)
    filter_limit_max: $(arg MAXIMUM_Z)  ## as same as robot height?
    filter_limit_negative: False
  </rosparam>

</launch>
