rm old «filtre» node
This commit is contained in:
Louis-Guillaume DUBOIS
parent
810663ee90
commit
fd3d1e4da0
2 changed files with 0 additions and 237 deletions
|
|
@ -1,121 +0,0 @@
|
||||||
#include <ros/ros.h>
|
|
||||||
#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;
|
|
||||||
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
uint8_t min, max, c;
|
|
||||||
|
|
||||||
//Recherche du min et du max des coefficients R,G,B
|
|
||||||
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;
|
|
||||||
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
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<PointCloud>("output", 1);
|
|
||||||
Callback callback(publisher, (int) hue, (int) delta);
|
|
||||||
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
|
|
||||||
|
|
||||||
// démarrage
|
|
||||||
ROS_INFO("node started");
|
|
||||||
ros::spin();
|
|
||||||
ROS_INFO("exit");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
@ -1,116 +0,0 @@
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <pcl_ros/point_cloud.h>
|
|
||||||
#include <pcl/point_types.h>
|
|
||||||
#include <pcl/filters/passthrough.h>
|
|
||||||
|
|
||||||
typedef pcl::PointXYZRGB Point;
|
|
||||||
typedef pcl::PointCloud<Point> 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 (abs(pt.r-red) <= delta and abs(pt.g-green) <= delta and abs(pt.b-blue) <= delta)
|
|
||||||
pcl->push_back(pt);
|
|
||||||
}
|
|
||||||
pcl->height = 1;
|
|
||||||
pcl->width = pcl->points.size();
|
|
||||||
publisher.publish(pcl);
|
|
||||||
}
|
|
||||||
|
|
||||||
Callback(ros::Publisher& pub, uint8_t r, uint8_t g, uint8_t b, uint8_t d)
|
|
||||||
: publisher(pub), delta(d), red(r), green(g), blue(b) {}
|
|
||||||
|
|
||||||
private:
|
|
||||||
ros::Publisher publisher;
|
|
||||||
uint8_t delta;
|
|
||||||
|
|
||||||
uint8_t red;
|
|
||||||
uint8_t green;
|
|
||||||
uint8_t blue;
|
|
||||||
|
|
||||||
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
|
|
||||||
uint8_t red(0);
|
|
||||||
uint8_t blue(0);
|
|
||||||
uint8_t green(0);
|
|
||||||
|
|
||||||
uint8_t delta(0);
|
|
||||||
|
|
||||||
int redInt;
|
|
||||||
int greenInt;
|
|
||||||
int blueInt;
|
|
||||||
|
|
||||||
int deltaInt;
|
|
||||||
|
|
||||||
if (node.getParam("red", redInt))
|
|
||||||
{
|
|
||||||
ROS_INFO("red : %d" , redInt);
|
|
||||||
} else {
|
|
||||||
node.setParam("red", 0);
|
|
||||||
node.getParam("red", redInt);
|
|
||||||
ROS_INFO("red : %d (default value)", redInt);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (node.getParam("blue", blueInt))
|
|
||||||
{
|
|
||||||
|
|
||||||
ROS_INFO("blue : %d" , blueInt);
|
|
||||||
} else {
|
|
||||||
node.setParam("blue", 0);
|
|
||||||
node.getParam("blue", blueInt);
|
|
||||||
ROS_INFO("blue : %d (default value)", blueInt);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (node.getParam("green", greenInt))
|
|
||||||
{
|
|
||||||
ROS_INFO("green : %d" , greenInt);
|
|
||||||
} else {
|
|
||||||
node.setParam("green", 0);
|
|
||||||
node.getParam("green", greenInt);
|
|
||||||
ROS_INFO("green : %d (default value)", greenInt);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (node.getParam("delta", deltaInt))
|
|
||||||
{
|
|
||||||
ROS_INFO("delta : %d" , deltaInt);
|
|
||||||
} else {
|
|
||||||
node.setParam("delta", 0);
|
|
||||||
node.getParam("delta", deltaInt);
|
|
||||||
ROS_INFO("delta : %d (default value)", deltaInt);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// initialisatio
|
|
||||||
ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
|
|
||||||
Callback callback(publisher, (uint8_t) redInt, (uint8_t) greenInt, (uint8_t) blueInt, (uint8_t) deltaInt);
|
|
||||||
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
|
|
||||||
|
|
||||||
// démarrage
|
|
||||||
ROS_INFO("node started");
|
|
||||||
ros::spin();
|
|
||||||
ROS_INFO("exit");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
Loading…
Reference in a new issue