From 31638974d8f7766a0b035986a70bd2edc03d19b9 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Tue, 26 May 2015 15:52:30 +0200
Subject: [PATCH 1/3] change function signatures in filtre.cpp

---
 src/filtre.cpp | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/src/filtre.cpp b/src/filtre.cpp
index 1de7d30..743be8e 100644
--- a/src/filtre.cpp
+++ b/src/filtre.cpp
@@ -27,7 +27,7 @@ class Callback {
       publisher.publish(pcl);
     }
 
-    Callback(ros::Publisher& pub)
+    Callback(const ros::Publisher& pub)
     : publisher(pub), z_max(90.), hue(0.), delta_hue(20.),  sat_min(0.3), sat_max(1.), val_min(0.3), val_max(1.)
     {
       assert(delta_hue > 0);
@@ -41,7 +41,7 @@ class Callback {
     }
 
   void
-  reconfigure(hand_control::FiltreConfig& c, uint32_t level) { 
+  reconfigure(const hand_control::FiltreConfig& c, const uint32_t level) { 
     z_max = c.z_max;
     hue = c.hue;
     delta_hue = c.delta_hue;
@@ -80,15 +80,19 @@ class Callback {
         diff2 = std::fabs(360.0f + hue - h);
       return std::min(diff1, diff2);
     }
-
-    float sat(const Point& pt)
+    
+    inline
+    float
+    sat(const Point& pt)
     {
       float h, s, v, diff1, diff2;
       pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
       return s;
     }
 
-    float val(const Point& pt)
+    inline
+    float
+    val(const Point& pt)
     {
       float h, s, v, diff1, diff2;
       pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);

From ed7e054c15c00c97daad81fe7c78ed3aead6a1b0 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Tue, 26 May 2015 16:03:42 +0200
Subject: [PATCH 2/3] better hsv computing code in filtre.cpp

---
 src/filtre.cpp | 42 +++++++++++++++++-------------------------
 1 file changed, 17 insertions(+), 25 deletions(-)

diff --git a/src/filtre.cpp b/src/filtre.cpp
index 743be8e..232d905 100644
--- a/src/filtre.cpp
+++ b/src/filtre.cpp
@@ -19,7 +19,9 @@ class Callback {
       copy_info(msg, pcl);
       BOOST_FOREACH (const Point& pt, msg->points)
       {
-        if (pt.z < z_max and hue_dist(pt) < delta_hue and sat(pt) < sat_max and sat(pt) > sat_min and val(pt) < val_max and val(pt) > val_min)
+        float hue_dist, sat, val;
+        hdist_s_v(pt, hue_dist, sat, val);
+        if (pt.z < z_max and hue_dist < delta_hue and sat < sat_max and sat > sat_min and val < val_max and val > val_min)
           pcl->push_back(pt);
       }
       pcl->height = 1;
@@ -41,7 +43,7 @@ class Callback {
     }
 
   void
-  reconfigure(const hand_control::FiltreConfig& c, const uint32_t level) { 
+  reconfigure(const hand_control::FiltreConfig& c, const uint32_t& level) { 
     z_max = c.z_max;
     hue = c.hue;
     delta_hue = c.delta_hue;
@@ -49,6 +51,14 @@ class Callback {
     val_max = c.val_max;
     sat_min = c.sat_min;
     sat_max = c.sat_max;
+    assert(delta_hue > 0);
+    assert(z_max > 0);
+    assert(hue >= 0);
+    assert(hue <= 360.);
+    assert(sat_min >= 0);
+    assert(sat_max <= 1.);
+    assert(val_min >= 0);
+    assert(val_max <= 1.);
   }
 
   private:
@@ -58,7 +68,7 @@ class Callback {
     inline
     void
     copy_info(const PointCloud::ConstPtr& a,
-              PointCloud::Ptr b)
+              PointCloud::Ptr& b)
     {
       b->header = a->header;
       b->sensor_origin_ = a->sensor_origin_;
@@ -67,10 +77,10 @@ class Callback {
     }
 
     inline
-    float
-    hue_dist(const Point& pt)
+    void
+    hdist_s_v(const Point& pt, float& h_dist, float& s, float& v)
     {
-      float h, s, v, diff1, diff2;
+      float h, diff1, diff2;
       pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
       h *= 360.0f ;
       diff1 = std::fabs(h - hue);
@@ -78,25 +88,7 @@ class Callback {
         diff2 = std::fabs(360.0f + h - hue);
       else
         diff2 = std::fabs(360.0f + hue - h);
-      return std::min(diff1, diff2);
-    }
-    
-    inline
-    float
-    sat(const Point& pt)
-    {
-      float h, s, v, diff1, diff2;
-      pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
-      return s;
-    }
-
-    inline
-    float
-    val(const Point& pt)
-    {
-      float h, s, v, diff1, diff2;
-      pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
-      return v;
+      h_dist = std::min(diff1, diff2);
     }
 };
 

From 63f23e27b4f25bd7e426e31f73d08be69b176aac Mon Sep 17 00:00:00 2001
From: Paul Janin <Paul.Janin@supelec.fr>
Date: Wed, 27 May 2015 15:50:05 +0200
Subject: [PATCH 3/3] Vertical command with z value

---
 CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 810854c..387a761 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -64,6 +64,7 @@ target_link_libraries(commande-new-1d ${catkin_LIBRARIES})
 #add dynamic reconfigure api
   generate_dynamic_reconfigure_options(
                                        cfg/Filtre.cfg
+				       cfg/Commande.cfg
                                       )
 add_dependencies(filtre ${PROJECT_NAME}_gencfg)
 catkin_package(