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

comments filter.cpp

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-06-26 20:40:20 +02:00
parent 3acd745d19
commit a70483de20
1 changed files with 12 additions and 5 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

17
src/filter.cpp
View file

@ -14,9 +14,11 @@ class Callback {
public: public:
void void
callback(const PointCloud::ConstPtr& msg) callback(const PointCloud::ConstPtr& msg)
// handles and filters the received PointCloud and
// publishes the filtered PointCloud
{ {
PointCloud::Ptr pcl(new PointCloud()); PointCloud::Ptr pcl(new PointCloud());
copy_info(msg, pcl); copy_info(msg, pcl); // copy the header
BOOST_FOREACH (const Point& pt, msg->points) BOOST_FOREACH (const Point& pt, msg->points)
{ {
float hue_dist, sat, val; float hue_dist, sat, val;
@ -34,7 +36,9 @@ class Callback {
{} {}
void void
reconfigure(const hand_control::FilterConfig& c, const uint32_t& level) { reconfigure(const hand_control::FilterConfig& c, const uint32_t& level)
// updates the parameters
{
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;
@ -52,6 +56,7 @@ class Callback {
void void
copy_info(const PointCloud::ConstPtr& a, copy_info(const PointCloud::ConstPtr& a,
PointCloud::Ptr& b) PointCloud::Ptr& b)
// copy the header info (useful in order to use rviz)
{ {
b->header = a->header; b->header = a->header;
b->sensor_origin_ = a->sensor_origin_; b->sensor_origin_ = a->sensor_origin_;
@ -62,11 +67,14 @@ class Callback {
inline inline
void void
hdist_s_v(const Point& pt, float& h_dist, float& s, float& v) hdist_s_v(const Point& pt, float& h_dist, float& s, float& v)
// calculate the distance from the wished hue, the saturation and the value
// of the point
{ {
float h, 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);
// hue is periodic
if (h < hue) if (h < hue)
diff2 = std::fabs(360.0f + h - hue); diff2 = std::fabs(360.0f + h - hue);
else else
@ -80,18 +88,17 @@ main(int argc, char** argv)
{ {
ros::init(argc, argv, "filter"); ros::init(argc, argv, "filter");
ros::NodeHandle node("filter"); ros::NodeHandle node("filter");
// initialisation
ros::Publisher publisher = node.advertise<PointCloud>("output", 1); ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
Callback my_callback(publisher); Callback my_callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &my_callback); ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, &Callback::callback, &my_callback);
// sets up dynamic_reconfigure
dynamic_reconfigure::Server<hand_control::FilterConfig> server; dynamic_reconfigure::Server<hand_control::FilterConfig> server;
dynamic_reconfigure::Server<hand_control::FilterConfig>::CallbackType f; dynamic_reconfigure::Server<hand_control::FilterConfig>::CallbackType f;
f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2); f = boost::bind(&Callback::reconfigure, &my_callback, _1, _2);
server.setCallback(f); server.setCallback(f);
// démarrage // begins working
ROS_INFO("node started"); ROS_INFO("node started");
ros::spin(); ros::spin();
ROS_INFO("exit"); ROS_INFO("exit");