diff --git a/CMakeLists.txt b/CMakeLists.txt index a5f9ae7..9701f71 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,6 +53,9 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR add_executable(filtreRGB src/filtreRGB.cpp) target_link_libraries(filtreRGB ${catkin_LIBRARIES}) +add_executable(filtreHue src/filtreHue.cpp) +target_link_libraries(filtreHue ${catkin_LIBRARIES}) + add_executable(normal_estimator src/normal_estimator.cpp) target_link_libraries(normal_estimator ${catkin_LIBRARIES}) add_dependencies(normal_estimator hand_control_generate_messages_cpp) diff --git a/src/filtreHue.cpp b/src/filtreHue.cpp new file mode 100644 index 0000000..4b4098e --- /dev/null +++ b/src/filtreHue.cpp @@ -0,0 +1,108 @@ +#include +#include +#include +#include + +typedef pcl::PointXYZRGB Point; +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) + { + int h(0); + float rScaled(pt.r/255); + float gScaled(pt.g/255); + float bScaled(pt.b/255); + + float cMax(std::max(std::max(rScaled, gScaled), bScaled)); + float cMin(std::min(std::min(rScaled, gScaled), bScaled)); + float cDelta(cMin-cMax); + + 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); + } + + if (h < 0) { + h += 360; + } + + if (abs(h - hue) < delta) { + pcl->push_back(pt); + } + } + 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, "filtreRGB"); + ros::NodeHandle node("filtreRGB"); + + // 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; +}