From f59b1246584020ce6ff42d6a41fbb5595a9aa871 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Wed, 22 Apr 2015 21:38:54 +0200
Subject: [PATCH] filtrage selon z

---
 hand_control/CMakeLists.txt               |  3 ++
 hand_control/src/filtre.cpp               | 54 ++++++++++++++-----
 hand_control/src/random_pcl_publisher.cpp | 63 +++++++++++++++++++++++
 3 files changed, 107 insertions(+), 13 deletions(-)

diff --git a/hand_control/CMakeLists.txt b/hand_control/CMakeLists.txt
index db4b01d..87e7100 100644
--- a/hand_control/CMakeLists.txt
+++ b/hand_control/CMakeLists.txt
@@ -112,6 +112,9 @@ add_executable(filtre src/filtre.cpp)
 target_link_libraries(filtre ${catkin_LIBRARIES})
 add_dependencies(filtre ${catkin_EXPORTED_TARGETS})
 
+add_executable(random_pcl_publisher src/random_pcl_publisher.cpp)
+target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES})
+add_dependencies(random_pcl_publisher ${catkin_EXPORTED_TARGETS})
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
 # add_dependencies(hand_control_node hand_control_generate_messages_cpp)
diff --git a/hand_control/src/filtre.cpp b/hand_control/src/filtre.cpp
index 8529eee..c7fa069 100644
--- a/hand_control/src/filtre.cpp
+++ b/hand_control/src/filtre.cpp
@@ -1,36 +1,64 @@
 #include <ros/ros.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/filters/passthrough.h>
 
 typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
 
 class Callback {
   public:
-    void operator()(const PointCloud::ConstPtr& msg)
-      {
-        ROS_INFO("PointCloud received");
-        // pour publier un shared_ptr (mieux)
-        PointCloud::Ptr pcl;
-        // copie du nuage de point
-        *pcl = *msg;
-        // TODO : ôter les mauvais points
-        publisher.publish(pcl);
-        ROS_INFO("PointCloud published");
-      }
+    void
+    operator()(const PointCloud::ConstPtr& msg)
+    {
+      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("zmax", zmax);
+      z_filtre.setFilterLimits(0.0, zmax);
+      z_filtre.setInputCloud(pcl);
+      z_filtre.filter(*pcl);
+      // publication
+      publisher.publish(pcl);
+      ROS_INFO("PointCloud published");
+    }
 
-    Callback(ros::Publisher& pub) : publisher(pub) {}
+    Callback(ros::Publisher& pub) : publisher(pub), z_filtre()
+    {
+      z_filtre.setFilterFieldName("z");
+    }
 
   private:
     ros::Publisher publisher;
+    pcl::PassThrough<pcl::PointXYZRGB> z_filtre;
 };
 
-int main(int argc, char** argv)
+int
+main(int argc, char** argv)
 {
   ros::init(argc, argv, "filtre");
   ros::NodeHandle node;
+
+  // récupération des paramètres
+  double zmax(0);
+  if (node.getParam("zmax", zmax))
+  {
+    ROS_INFO("zmax : %f" , zmax);
+  } else {
+    node.setParam("zmax", 50.0);
+    node.getParam("zmax", zmax);
+    ROS_INFO("zmax : %f (default value)", zmax);
+  }
+
+  // initialisation
   ros::Publisher  publisher = node.advertise<PointCloud>("filtre_output", 1);
   Callback callback(publisher);
   ros::Subscriber subscriber = node.subscribe<PointCloud>("filtre_input", 1, callback);
+
+  // démarrage
   ROS_INFO("node started");
   ros::spin();
   ROS_INFO("exit");
diff --git a/hand_control/src/random_pcl_publisher.cpp b/hand_control/src/random_pcl_publisher.cpp
index e69de29..88089f7 100644
--- a/hand_control/src/random_pcl_publisher.cpp
+++ b/hand_control/src/random_pcl_publisher.cpp
@@ -0,0 +1,63 @@
+#include <ros/ros.h>
+#include <pcl_ros/point_cloud.h>
+#include <pcl/point_types.h>
+// for UniformGenerator
+#include <pcl/common/random.h>
+// for CloudGenerator
+#include <pcl/common/generate.h>
+
+typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
+typedef pcl::common::UniformGenerator<float> UGenerator;
+
+class Generator
+{
+  public:
+    Generator(int l) : length(l), cgen()
+    {
+      UGenerator::Parameters params(-100, 100,-1);
+      cgen.setParameters(params);
+    }
+
+    PointCloud::Ptr
+    operator()()
+    {
+      PointCloud::Ptr pcl(new PointCloud());
+      cgen.fill(length, length, *pcl);
+      ROS_INFO("random cloud :");
+      for(int i = 0; i < pcl->points.size(); ++i)
+      {
+        ROS_INFO("\nx : %f\ny : %f\nz : %f\nr : %d\ng : %d\nb : %d",
+                pcl->points[i].x,
+                pcl->points[i].y,
+                pcl->points[i].z,
+                pcl->points[i].r,
+                pcl->points[i].g,
+                pcl->points[i].b);
+      }
+      return pcl;
+    }
+
+  private:
+    pcl::common::CloudGenerator<pcl::PointXYZRGB, UGenerator> cgen;
+    int length;
+};
+
+int
+main(int argc, char** argv)
+{
+  ros::init(argc, argv, "random_pcl_publisher");
+  ros::NodeHandle node;
+  ros::Publisher  publisher = node.advertise<PointCloud>("random_output", 1);
+  Generator generator(5);
+  ros::Rate loop_rate(0.5);
+  ROS_INFO("node started");
+  while (ros::ok())
+  {
+    publisher.publish(generator());
+    ROS_INFO("random PointCloud published");
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+  ROS_INFO("exit");
+  return 0;
+}