<launch>
  <node pkg="jsk_footstep_planner" type="footstep_planner_node" name="footstep_planner" output="screen">
    <remap from="~pointcloud_model" to="pointcloud_model_generator/output" />
    <remap from="~obstacle_model" to="octomap_point_cloud_centers" />
    <rosparam>
      use_pointcloud_model: true
      use_lazy_perception: true
      use_local_movement: true
      project_start_state: true
      project_goal_state: true
      #use_obstacle_model: false
      use_obstacle_model: true
      obstacle_resolution: 0.1
    </rosparam>
    <rosparam>
      footstep_size_x: 0.2
      footstep_size_y: 0.1
      collision_bbox_size: [0.4, 1, 1.0]
      collision_bbox_offset: [0, 0, 0.8]
    </rosparam>
    <!-- successors -->
    <rosparam>
      successors:
      - x: 0
        y: -0.2
        theta: 0
      - x: 0
        y: -0.3
        theta: 0
      - x: 0
        y: -0.15
        theta: 0
      - x: 0.2
        y: -0.2
        theta: 0
      - x: 0.25
        y: -0.2
        theta: 0
      - x: 0.3
        y: -0.2
        theta: 0
      - x: 0.1
        y: -0.2
        theta: 0
      - x: -0.1
        y: -0.2
        theta: 0
      - x: 0
        y: -0.2
        theta: 0.17
      - x: 0
        y: -0.3
        theta: 0.17
      - x: 0.2
        y: -0.2
        theta: 0.17
      - x: 0.25
        y: -0.2
        theta: 0.17
      - x: 0.1
        y: -0.2
        theta: 0.17
      - x: 0
        y: -0.2
        theta: -0.17
      - x: 0
        y: -0.3
        theta: -0.17
      - x: 0.2
        y: -0.2
        theta: -0.17
      - x: 0.25
        y: -0.2
        theta: -0.17
      - x: 0.1
        y: -0.2
        theta: -0.17
    </rosparam>
  </node>
  <node pkg="jsk_footstep_planner" type="footstep_marker" name="footstep_marker" output="screen">
    <rosparam>
      disable_tf: true
      footstep_size_x: 0.237418
      footstep_size_y: 0.134017
      lleg_footstep_offset: [0.015502, 0.010078, 0.0]
      rleg_footstep_offset: [0.015502, -0.010078, 0.0]
    </rosparam>
  </node>

  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <param name="resolution" value="0.1" />
    <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
    <param name="frame_id" type="string" value="odom" />
    
    <!-- maximum range to integrate (speedup!) -->
    <param name="sensor_model/max_range" value="5.0" />
    
    <!-- data source to integrate (PointCloud2) -->
    <remap from="cloud_in" to="pointcloud_model_generator/output" />
    
  </node>

  <node pkg="jsk_footstep_planner" type="pointcloud_model_generator_node"
        name="pointcloud_model_generator">
  </node>
  <node pkg="rviz" type="rviz" name="rviz"
        args="-d $(find jsk_footstep_planner)/launch/sample/footstep_planner_perception_sample.rviz" />
</launch>
