72 lines
1.6 KiB
C++
72 lines
1.6 KiB
C++
#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 (pt.z < zmax)
|
|
pcl->push_back(pt);
|
|
}
|
|
pcl->height = 1;
|
|
pcl->width = pcl->points.size();
|
|
publisher.publish(pcl);
|
|
}
|
|
|
|
Callback(ros::Publisher& pub, float z)
|
|
: publisher(pub), zmax(z) {}
|
|
|
|
private:
|
|
ros::Publisher publisher;
|
|
float zmax;
|
|
|
|
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, "filtre");
|
|
ros::NodeHandle node("filtre");
|
|
|
|
// récupération des paramètres
|
|
double zmax(0);
|
|
if (node.getParam("zmax", zmax))
|
|
{
|
|
ROS_INFO("zmax : %f" , zmax);
|
|
} else {
|
|
node.setParam("zmax", 50.0);
|
|
node.getParam("zmax", zmax);
|
|
ROS_INFO("zmax : %f (default value)", zmax);
|
|
}
|
|
|
|
// initialisatio
|
|
ros::Publisher publisher = node.advertise<PointCloud>("output", 1);
|
|
Callback callback(publisher, (float) zmax);
|
|
ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
|
|
|
|
// démarrage
|
|
ROS_INFO("node started");
|
|
ros::spin();
|
|
ROS_INFO("exit");
|
|
return 0;
|
|
}
|