#include #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) { 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) pcl->push_back(pt); } pcl->height = 1; pcl->width = pcl->points.size(); publisher.publish(pcl); } Callback(ros::Publisher& pub, float z, float h, float delta_h, float smin, float smax, float vmin, float vmax) : publisher(pub), z_max(z), hue(h), delta_hue(delta_h), sat_min(smin), sat_max(smax), val_min(vmin), val_max(vmax) { 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: ros::Publisher publisher; float z_max, hue, delta_hue, val_min, val_max, sat_min, sat_max; 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; } inline float hue_dist(const Point& pt) { float h, s, v, diff1, diff2; pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v); h *= 360.0f ; diff1 = std::fabs(h - hue); if (h < hue) diff2 = std::fabs(360.0f + h - hue); else diff2 = std::fabs(360.0f + hue - h); return std::min(diff1, diff2); } 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) { float h, s, v, diff1, diff2; pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v); return v; } }; int main(int argc, char** argv) { ros::init(argc, argv, "filtre"); ros::NodeHandle node("filtre"); // récupération des paramètres double z_max(0); if (node.getParam("z_max", z_max)) { ROS_INFO("z_max : %f" , z_max); } else { node.setParam("z_max", 50.0); node.getParam("z_max", z_max); ROS_INFO("z_max : %f (default value)", z_max); } double hue(0); if (node.getParam("hue", hue)) { ROS_INFO("hue : %f" , hue); } else { node.setParam("hue", 0.0); node.getParam("hue", hue); ROS_INFO("hue : %f (default value)", hue); } double delta_hue(0); if (node.getParam("delta_hue", delta_hue)) { ROS_INFO("delta_hue : %f" , delta_hue); } else { node.setParam("delta_hue", 10.0); node.getParam("delta_hue", delta_hue); ROS_INFO("delta_hue : %f (default value)", delta_hue); } double sat_min(0); if (node.getParam("sat_min", sat_min)) { ROS_INFO("sat_min : %f" , sat_min); } else { node.setParam("sat_min", 0.1); node.getParam("sat_min", sat_min); ROS_INFO("sat_min : %f (default value)", sat_min); } double sat_max(0); if (node.getParam("sat_max", sat_max)) { ROS_INFO("sat_max : %f" , sat_max); } else { node.setParam("sat_max", 1.); node.getParam("sat_max", sat_max); ROS_INFO("sat_max : %f (default value)", sat_max); } double val_min(0); if (node.getParam("val_min", val_min)) { ROS_INFO("val_min : %f" , val_min); } else { node.setParam("val_min", 0.4); node.getParam("val_min", val_min); ROS_INFO("val_min : %f (default value)", val_min); } double val_max(0); if (node.getParam("val_max", val_max)) { ROS_INFO("val_max : %f" , val_max); } else { node.setParam("val_max", 1.); node.getParam("val_max", val_max); ROS_INFO("val_max : %f (default value)", val_max); } // initialisatio ros::Publisher publisher = node.advertise("output", 1); Callback callback(publisher, (float) z_max, (float) hue, (float) delta_hue, (float) sat_min, (float) sat_max, (float) val_min, (float) val_max); ros::Subscriber subscriber = node.subscribe("input", 1, callback); // démarrage ROS_INFO("node started"); ros::spin(); ROS_INFO("exit"); return 0; }