From 74386e1a9f7de30bd6e56db381c5b14fabd1cc61 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Thu, 30 Apr 2015 17:23:17 +0200
Subject: [PATCH] try : better filtre ?

---
 src/filtre.cpp | 16 ++++++----------
 1 file changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/filtre.cpp b/src/filtre.cpp
index 9f77ba0..fca30d7 100644
--- a/src/filtre.cpp
+++ b/src/filtre.cpp
@@ -13,28 +13,24 @@ class Callback {
       ROS_INFO("PointCloud received");
       // pour publier un shared_ptr (mieux)
       PointCloud::Ptr pcl(new PointCloud());
-      // copie du nuage de point
-      *pcl = *msg;
       // filtrage
       double zmax(0.);
       ros::param::getCached("/filtre/zmax", zmax);
       ROS_INFO("zmax : %f", zmax);
-      z_filtre.setFilterLimits(0.0, zmax);
-      z_filtre.setInputCloud(pcl);
-      z_filtre.filter(*pcl);
+      for (int i = 0; i < msg->points.size(); ++i)
+      {
+        if (msg->points[i].z > zmax)
+          pcl->points.push_back(msg->points[i]);
+      }
       // publication
       publisher.publish(pcl);
       ROS_INFO("PointCloud published");
     }
 
-    Callback(ros::Publisher& pub) : publisher(pub), z_filtre()
-    {
-      z_filtre.setFilterFieldName("z");
-    }
+    Callback(ros::Publisher& pub) : publisher(pub) {}
 
   private:
     ros::Publisher publisher;
-    pcl::PassThrough<pcl::PointXYZRGB> z_filtre;
 };
 
 int