From 72a18c61747b543cd38a773e8f97e0c52770187b Mon Sep 17 00:00:00 2001
From: YoheiKakiuchi <youhei@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 27 Sep 2016 22:50:17 +0900
Subject: [PATCH] [jsk_footstep_planner] update launch for JAXON_RED

---
 .../heightmap_perception/heightmap.launch     | 67 ++++++++++++++-----
 1 file changed, 51 insertions(+), 16 deletions(-)

diff --git a/jsk_footstep_planner/launch/heightmap_perception/heightmap.launch b/jsk_footstep_planner/launch/heightmap_perception/heightmap.launch
index 14eb4f251..554c1bece 100644
--- a/jsk_footstep_planner/launch/heightmap_perception/heightmap.launch
+++ b/jsk_footstep_planner/launch/heightmap_perception/heightmap.launch
@@ -7,6 +7,7 @@
   <arg name="ROBOT_FRAME" default="body_on_odom" /> <!--it's not good-->
   <arg name="INPUT" default="/distance_filtered_laser_snapshots/output_cloud" />
   <arg name="DEBUG_VIEW" default="true" />
+
   <node pkg="jsk_footstep_controller" type="robot-boundingbox.l" name="robot_bounding_box">
     <rosparam>
       analysis_level: whole_links
@@ -91,7 +92,6 @@
   <rosparam param="map_origin_polygon" subst_value="true">
     use_message: true
     frame_ids: [$(arg STAND_FRAME)]
-    #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]]]
     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]]]
   </rosparam>
   <rosparam param="robot_center_map_origin_points" subst_value="true">
@@ -106,12 +106,17 @@
     max_queue_size: 50
   </rosparam>
   <rosparam param="latest_heightmap" subst_value="true">
-    min_x: -4.0
-    max_x: 4.0
+    min_x: -3.0
+    max_x:  5.0
     min_y: -4.0
-    max_y: 4.0
-    resolution_x: 800
-    resolution_y: 800
+    max_y:  4.0
+    resolution_x: 800 ## 0.01 m/pixel
+    resolution_y: 800 ## 0.01 m/pixel
+  </rosparam>
+  <rosparam param="latest_complete_heightmap" subst_value="true">
+    mask_size: 4 ## cover 0.08m
+    max_variance: 0.14
+    smooth_meshod: average
   </rosparam>
   <rosparam param="accumulated_heightmap" subst_value="true">
     center_frame_id: $(arg ROBOT_FRAME)
@@ -127,32 +132,62 @@
   <rosparam param="robot_center_pointcloud_bbox_clipped" subst_value="true">
     negative: true
   </rosparam>
-  <group ns="heightmap_visualization">
-    <group ns="latest">
+  <group ns="heightmap_visualization" if="$(arg DEBUG_VIEW)" >
+    <group ns="latest_map">
+      <node pkg="jsk_perception" type="colorize_float_image" name="colorize_heightmap">
+        <remap from="~input" to="/latest_heightmap/output" />
+      </node>
+      <node pkg="image_view" type="image_view" name="heightmap_view">
+        <remap from="image" to="colorize_heightmap/output" />
+        <param name="autosize" value="true" />
+      </node>
+    </group>
+    <group ns="filtered_map">
+      <node pkg="jsk_perception" type="colorize_float_image" name="colorize_heightmap">
+        <remap from="~input" to="/latest_complete_heightmap/output" />
+      </node>
+      <node pkg="image_view" type="image_view" name="heightmap_view" >
+        <remap from="image" to="colorize_heightmap/output" />
+        <param name="autosize" value="true" />
+      </node>
+    </group>
+    <group ns="accumulated_map">
+      <node pkg="jsk_perception" type="colorize_float_image" name="colorize_heightmap">
+        <remap from="~input" to="/accumulated_heightmap/output" />
+      </node>
+      <node pkg="image_view" type="image_view" name="heightmap_view" >
+        <remap from="image" to="colorize_heightmap/output" />
+        <param name="autosize" value="true" />
+      </node>
+    </group>
+    <group ns="latest_quality">
       <node pkg="jsk_perception" type="colorize_float_image" name="colorize_heightmap">
         <remap from="~input" to="/latest_heightmap/output" />
+        <param name="channel" value="1" />
       </node>
-      <node pkg="image_view" type="image_view" name="heightmap_view"
-            if="$(arg DEBUG_VIEW)">
+      <node pkg="image_view" type="image_view" name="heightmap_view" >
         <remap from="image" to="colorize_heightmap/output" />
+        <param name="autosize" value="true" />
       </node>
     </group>
-    <group ns="filtered">
+    <group ns="filtered_quality">
       <node pkg="jsk_perception" type="colorize_float_image" name="colorize_heightmap">
         <remap from="~input" to="/latest_complete_heightmap/output" />
+        <param name="channel" value="1" />
       </node>
-      <node pkg="image_view" type="image_view" name="heightmap_view"
-            if="$(arg DEBUG_VIEW)">
+      <node pkg="image_view" type="image_view" name="heightmap_view" >
         <remap from="image" to="colorize_heightmap/output" />
+        <param name="autosize" value="true" />
       </node>
     </group>
-    <group ns="accumulated">
+    <group ns="accumulated_quality">
       <node pkg="jsk_perception" type="colorize_float_image" name="colorize_heightmap">
         <remap from="~input" to="/accumulated_heightmap/output" />
+        <param name="channel" value="1" />
       </node>
-      <node pkg="image_view" type="image_view" name="heightmap_view"
-            if="$(arg DEBUG_VIEW)">
+      <node pkg="image_view" type="image_view" name="heightmap_view" >
         <remap from="image" to="colorize_heightmap/output" />
+        <param name="autosize" value="true" />
       </node>
     </group>
   </group>