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

Merge branch 'master' of https://bitbucket.org/_Luc_/hand_control

Browse source
This commit is contained in:
_Luc_ 2015-05-27 15:52:00 +02:00
commit e7c8fca8a2
2 changed files with 19 additions and 22 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
CMakeLists.txt
View file

@ -64,6 +64,7 @@ target_link_libraries(commande-new-1d ${catkin_LIBRARIES})
#add dynamic reconfigure api #add dynamic reconfigure api
generate_dynamic_reconfigure_options( generate_dynamic_reconfigure_options(
cfg/Filtre.cfg cfg/Filtre.cfg
cfg/Commande.cfg
) )
add_dependencies(filtre ${PROJECT_NAME}_gencfg) add_dependencies(filtre ${PROJECT_NAME}_gencfg)
catkin_package( catkin_package(

40
src/filtre.cpp
View file

@ -19,7 +19,9 @@ 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 < 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) float hue_dist, sat, val;
hdist_s_v(pt, hue_dist, sat, val);
if (pt.z < z_max and hue_dist < delta_hue and sat < sat_max and sat > sat_min and val < val_max and val > val_min)
pcl->push_back(pt); pcl->push_back(pt);
} }
pcl->height = 1; pcl->height = 1;
@ -27,7 +29,7 @@ class Callback {
publisher.publish(pcl); publisher.publish(pcl);
} }
Callback(ros::Publisher& pub) Callback(const ros::Publisher& pub)
: publisher(pub), z_max(90.), hue(0.), delta_hue(20.), sat_min(0.3), sat_max(1.), val_min(0.3), val_max(1.) : publisher(pub), z_max(90.), hue(0.), delta_hue(20.), sat_min(0.3), sat_max(1.), val_min(0.3), val_max(1.)
{ {
assert(delta_hue > 0); assert(delta_hue > 0);
@ -41,7 +43,7 @@ class Callback {
} }
void void
reconfigure(hand_control::FiltreConfig& c, uint32_t level) { reconfigure(const hand_control::FiltreConfig& c, const uint32_t& level) {
z_max = c.z_max; z_max = c.z_max;
hue = c.hue; hue = c.hue;
delta_hue = c.delta_hue; delta_hue = c.delta_hue;
@ -49,6 +51,14 @@ class Callback {
val_max = c.val_max; val_max = c.val_max;
sat_min = c.sat_min; sat_min = c.sat_min;
sat_max = c.sat_max; sat_max = c.sat_max;
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: private:
@ -58,7 +68,7 @@ class Callback {
inline inline
void void
copy_info(const PointCloud::ConstPtr& a, copy_info(const PointCloud::ConstPtr& a,
PointCloud::Ptr b) PointCloud::Ptr& b)
{ {
b->header = a->header; b->header = a->header;
b->sensor_origin_ = a->sensor_origin_; b->sensor_origin_ = a->sensor_origin_;
@ -67,10 +77,10 @@ class Callback {
} }
inline inline
float void
hue_dist(const Point& pt) hdist_s_v(const Point& pt, float& h_dist, float& s, float& v)
{ {
float h, s, v, diff1, diff2; float h, diff1, diff2;
pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v); pcl::tracking::RGB2HSV(pt.r, pt.g, pt.b, h, s, v);
h *= 360.0f ; h *= 360.0f ;
diff1 = std::fabs(h - hue); diff1 = std::fabs(h - hue);
@ -78,21 +88,7 @@ class Callback {
diff2 = std::fabs(360.0f + h - hue); diff2 = std::fabs(360.0f + h - hue);
else else
diff2 = std::fabs(360.0f + hue - h); diff2 = std::fabs(360.0f + hue - h);
return std::min(diff1, diff2); h_dist = 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;
} }
}; };