LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

attention commit nucléaire, risque d'implosion du dd

Browse source
This commit is contained in:
Paul Janin 2015-05-22 17:09:35 +02:00
parent 6b27426ac7
commit a202a4c19a
1 changed files with 63 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

68
src/filtre.cpp
View file

@ -17,7 +17,7 @@ class Callback {
copy_info(msg, pcl); copy_info(msg, pcl);
BOOST_FOREACH (const Point& pt, msg->points) BOOST_FOREACH (const Point& pt, msg->points)
{ {
if (pt.z < zmax and hue_dist(pt) < delta_hue) if (pt.z < zmax and hue_dist(pt) < delta_hue and sat_dist(pt) < delta_sat and val_dist(pt) < delta_val)
pcl->push_back(pt); pcl->push_back(pt);
} }
pcl->height = 1; pcl->height = 1;
@ -25,18 +25,22 @@ class Callback {
publisher.publish(pcl); publisher.publish(pcl);
} }
Callback(ros::Publisher& pub, float z, float h, float delta_h) Callback(ros::Publisher& pub, float z, float h, float delta_h, float s, float delta_s, float v, float delta_v)
: publisher(pub), zmax(z), hue(h), delta_hue(delta_h) : publisher(pub), zmax(z), hue(h), delta_hue(delta_h), sat(s), delta_sat(delta_s), val(v), delta_val(delta_val)
{ {
assert(delta_hue > 0); assert(delta_hue > 0);
assert(zmax > 0); assert(zmax > 0);
assert(hue >= 0); assert(hue >= 0);
assert(hue <= 360.); assert(hue <= 360.);
assert(sat >= 0);
assert(sat <= 1.);
assert(val >= 0);
assert(val <= 1.);
} }
private: private:
ros::Publisher publisher; ros::Publisher publisher;
float zmax, hue, delta_hue; float zmax, hue, delta_hue, val, delta_val, sat, delta_sat;
inline inline
void void
@ -63,6 +67,20 @@ class Callback {
diff2 = std::fabs(360.0f + hue - h); diff2 = std::fabs(360.0f + hue - h);
return std::min(diff1, diff2); return std::min(diff1, diff2);
} }
sat_dist(const Point& pt)
{
float h, s, v, diff1, diff2;
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
return std::fabs(s - sat);
}
val_dist(const Point& pt)
{
float h, s, v, diff1, diff2;
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
return std::fabs(v - val);
}
}; };
int int
@ -102,9 +120,49 @@ main(int argc, char** argv)
ROS_INFO("delta_hue : %f (default value)", delta_hue); ROS_INFO("delta_hue : %f (default value)", delta_hue);
} }
double sat(0);
if (node.getParam("sat", sat))
{
ROS_INFO("sat : %f" , sat);
} else {
node.setParam("sat", 0.0);
node.getParam("sat", sat);
ROS_INFO("sat : %f (default value)", sat);
}
double delta_sat(0);
if (node.getParam("delta_sat", delta_sat))
{
ROS_INFO("delta_sat : %f" , delta_sat);
} else {
node.setParam("delta_sat", 10.0);
node.getParam("delta_sat", delta_sat);
ROS_INFO("delta_sat : %f (default satue)", delta_sat);
}
double val(0);
if (node.getParam("val", hue))
{
ROS_INFO("val : %f" , hue);
} else {
node.setParam("val", 0.0);
node.getParam("val", hue);
ROS_INFO("val : %f (default value)", hue);
}
double delta_val(0);
if (node.getParam("delta_val", delta_hue))
{
ROS_INFO("delta_val : %f" , delta_hue);
} else {
node.setParam("delta_val", 10.0);
node.getParam("delta_val", delta_hue);
ROS_INFO("delta_val : %f (default value)", delta_hue);
}
// initialisatio // initialisatio
ros::Publisher publisher = node.advertise<PointCloud>("output", 1); ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
Callback callback(publisher, (float) zmax, (float) hue, (float) delta_hue); Callback callback(publisher, (float) zmax, (float) hue, (float) delta_hue, (float) sat, (float) delta_sat, (float) val, (float) delta_val);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback); ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
// démarrage // démarrage