costmap_cspace: fix frame of z-filter in pointcloud2_to_map. (#80)

at-wat · Oct 31, 2017 · ade7b5c · ade7b5c
1 parent 7156c80
commit ade7b5c
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/costmap_cspace/src/pointcloud2_to_map.cpp b/costmap_cspace/src/pointcloud2_to_map.cpp
@@ -67,7 +67,9 @@ class pointcloud2_to_map
 
 public:
   pointcloud2_to_map()
-    : nh(), pnh("~"), accums(2)
+    : nh()
+    , pnh("~")
+    , accums(2)
   {
     pnh.param("z_min", z_min, 0.1);
     pnh.param("z_max", z_max, 1.0);
@@ -134,6 +136,7 @@ class pointcloud2_to_map
       return;
     published = now;
 
+    float robot_z;
     try
     {
       tf::StampedTransform trans;
@@ -148,6 +151,7 @@ class pointcloud2_to_map
       map.info.origin.orientation.w = 1.0;
       origin_x = x - width * map.info.resolution * 0.5;
       origin_y = y - height * map.info.resolution * 0.5;
+      robot_z = pos.z();
     }
     catch (tf::TransformException &e)
     {
@@ -166,7 +170,7 @@ class pointcloud2_to_map
         sensor_msgs::PointCloud2Iterator<float> iter_z(pc, "z");
         for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
         {
-          if (*iter_z < z_min || z_max < *iter_z)
+          if (*iter_z - robot_z < z_min || z_max < *iter_z - robot_z)
             continue;
           unsigned int x = int(
               (*iter_x - map.info.origin.position.x) / map.info.resolution);