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

work on progress : filtreHue

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-06 21:00:14 +02:00
parent 1a17b44c7d
commit fd0bda6af7
2 changed files with 101 additions and 91 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
launch/all.launch
View file

@ -13,6 +13,5 @@
<remap from="/estimator/input" to="/filtre/output"/> <remap from="/estimator/input" to="/filtre/output"/>
</node> </node>
<include file="$(find hand_control)/launch/commande.launch"/>
</launch> </launch>

191
src/filtreHue.cpp
View file

@ -2,6 +2,7 @@
#include <pcl_ros/point_cloud.h> #include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/filters/passthrough.h> #include <pcl/filters/passthrough.h>
#include <assert.h>
typedef pcl::PointXYZRGB Point; typedef pcl::PointXYZRGB Point;
typedef pcl::PointCloud<Point> PointCloud; typedef pcl::PointCloud<Point> PointCloud;
@ -9,100 +10,110 @@ typedef pcl::PointCloud<Point> PointCloud;
class Callback { class Callback {
public: public:
void void
operator()(const PointCloud::ConstPtr& msg) operator()(const PointCloud::ConstPtr& msg)
{
PointCloud::Ptr pcl(new PointCloud());
copy_info(msg, pcl);
BOOST_FOREACH (const Point& pt, msg->points)
{ {
int h(0); PointCloud::Ptr pcl(new PointCloud());
float rScaled(pt.r/255); copy_info(msg, pcl);
float gScaled(pt.g/255); BOOST_FOREACH (const Point& pt, msg->points)
float bScaled(pt.b/255); {
uint8_t min, max, c;
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;
}
float cMax(std::max(std::max(rScaled, gScaled), bScaled)); c = max - min;
float cMin(std::min(std::min(rScaled, gScaled), bScaled));
float cDelta(cMin-cMax);
if (cMax == rScaled){ assert(c > 0);
h = (int) 60*(gScaled-bScaled)/cDelta; assert(max > pt.r);
}else if (cMax = gScaled){ assert(max > pt.g);
h = (int) 60*(2+(bScaled-rScaled)/cDelta); assert(max > pt.b);
}else if (cMax = rScaled){ assert(min < pt.r);
h = (int) 60*(4+(rScaled-gScaled)/cDelta); assert(min < pt.g);
assert(min < pt.b);
pcl->height = 1;
pcl->width = pcl->points.size();
publisher.publish(pcl);
} }
if (h < 0) { Callback(ros::Publisher& pub, int h, int d)
h += 360; : publisher(pub), delta(d), hue(h) {}
}
if (abs(h - hue) < delta) { private:
pcl->push_back(pt); 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;
} }
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;
}