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