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; -}