work on progress : filtreHue

launch/all.launch

@@ -13,6 +13,5 @@
     <remap from="/estimator/input" to="/filtre/output"/>
   </node>
 
-  <include file="$(find hand_control)/launch/commande.launch"/>
 
 </launch>

src/filtreHue.cpp

@@ -2,6 +2,7 @@
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/filters/passthrough.h>
+#include <assert.h>
 
 typedef pcl::PointXYZRGB Point;
 typedef pcl::PointCloud<Point> PointCloud;
@@ -9,100 +10,110 @@ typedef pcl::PointCloud<Point> PointCloud;
 class Callback {
   public:
     void
-    operator()(const PointCloud::ConstPtr& msg)
+      operator()(const PointCloud::ConstPtr& msg)
-    {
-      PointCloud::Ptr pcl(new PointCloud());
-      copy_info(msg, pcl);
-      BOOST_FOREACH (const Point& pt, msg->points)
       {
-        int h(0);
+        PointCloud::Ptr pcl(new PointCloud());
-	float rScaled(pt.r/255);
+        copy_info(msg, pcl);
-	float gScaled(pt.g/255);
+        BOOST_FOREACH (const Point& pt, msg->points)
-	float bScaled(pt.b/255);
+        {
-
+          uint8_t min, max, c;
-        float cMax(std::max(std::max(rScaled, gScaled), bScaled));
+          if (pt.r >= pt.g) {
-	float cMin(std::min(std::min(rScaled, gScaled), bScaled));
+            if (pt.g >= pt.b) {
-        float cDelta(cMin-cMax);
+              max = pt.r
-
+                min = pt.b;
-	if (cMax == rScaled){
+            } else if (pt.r >= pt.b) {
-	  h = (int) 60*(gScaled-bScaled)/cDelta;
+              max = pt.r;
-	}else if (cMax = gScaled){
+              min = pt.g;
-	  h = (int) 60*(2+(bScaled-rScaled)/cDelta);
+            } else {
-	}else if (cMax = rScaled){
+              max = pt.b;
-	  h = (int) 60*(4+(rScaled-gScaled)/cDelta);
+              min = pt.g;
+            }
+          } else if (pt.r >= pt.b) {
+            max = pt.g;
+            min = pt.b;
+          } else if (pt.g >= pt.b) {
+            max = pt.g;
+            min = pt.r;
+          } else {
+            max = pt.b;
+            min = pt.r;
+          }
+          
+          c = max - min;
+
+          assert(c > 0);
+          assert(max > pt.r);
+          assert(max > pt.g);
+          assert(max > pt.b);
+          assert(min < pt.r);
+          assert(min < pt.g);
+          assert(min < pt.b);
+         
+          
+
+          pcl->height = 1;
+          pcl->width = pcl->points.size();
+          publisher.publish(pcl);
         }
 
-	if (h < 0) {
+        Callback(ros::Publisher& pub, int h,  int d)
-	  h += 360;
+          : publisher(pub), delta(d), hue(h)  {}
-	}
+
-
+        private:
-	if (abs(h - hue) < delta) {
+        ros::Publisher publisher;
-	  pcl->push_back(pt);
+        int delta;
-	}
+
-      }
+        int hue;
-      pcl->height = 1;
+
-      pcl->width = pcl->points.size();
+        inline
-      publisher.publish(pcl);
+          void
-    }
+          copy_info(const PointCloud::ConstPtr& a,
-
+                    PointCloud::Ptr b)
-    Callback(ros::Publisher& pub, int h,  int d)
+          {
-    : publisher(pub), delta(d), hue(h)  {}
+            b->header = a->header;
-
+            b->sensor_origin_ = a->sensor_origin_;
-  private:
+            b->sensor_orientation_ = a->sensor_orientation_;
-    ros::Publisher publisher;
+            b->is_dense = a->is_dense;
-    int delta;
+          }
-
+      };
-    int hue;
+
-
+    int
-    inline
+      main(int argc, char** argv)
-    void
+      {
-    copy_info(const PointCloud::ConstPtr& a,
+        ros::init(argc, argv, "filtreHue");
-              PointCloud::Ptr b)
+        ros::NodeHandle node("filtreHue");
-    {
+
-      b->header = a->header;
+        // récupération des paramètres
-      b->sensor_origin_ = a->sensor_origin_;
+        int hue(0);  
-      b->sensor_orientation_ = a->sensor_orientation_;
+        int delta(0);
-      b->is_dense = a->is_dense;
+
-    }
+        if (node.getParam("hue", hue))
-};
+        {
-
+          ROS_INFO("hue : %d" , hue);
-int
+        } else {
-main(int argc, char** argv)
+          node.setParam("hue", 0);
-{
+          node.getParam("hue", hue);
-  ros::init(argc, argv, "filtreHue");
+          ROS_INFO("hue : %d (default value)", hue);
-  ros::NodeHandle node("filtreHue");
+        }
-
-  // récupération des paramètres
-  int hue(0);  
-  int delta(0);
 
-  if (node.getParam("hue", hue))
+        if (node.getParam("delta", delta))
-  {
+        {
-    ROS_INFO("hue : %d" , hue);
+          ROS_INFO("delta : %d" , delta);
-  } else {
+        } else {
-    node.setParam("hue", 0);
+          node.setParam("delta", 0);
-    node.getParam("hue", hue);
+          node.getParam("delta", delta);
-    ROS_INFO("hue : %d (default value)", hue);
+          ROS_INFO("delta : %d (default value)", delta);
-  }
+        }
 
-  if (node.getParam("delta", delta))
-  {
-    ROS_INFO("delta : %d" , delta);
-  } else {
-    node.setParam("delta", 0);
-    node.getParam("delta", delta);
-    ROS_INFO("delta : %d (default value)", delta);
-  }
- 
 
-  // initialisatio
+        // initialisatio
-  ros::Publisher  publisher = node.advertise<PointCloud>("output", 1);
+        ros::Publisher  publisher = node.advertise<PointCloud>("output", 1);
-  Callback callback(publisher, (int) hue, (int) delta);
+        Callback callback(publisher, (int) hue, (int) delta);
-  ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
+        ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
 
-  // démarrage
+        // démarrage
-  ROS_INFO("node started");
+        ROS_INFO("node started");
-  ros::spin();
+        ros::spin();
-  ROS_INFO("exit");
+        ROS_INFO("exit");
-  return 0;
+        return 0;
-}
+      }