filtrage selon z

This commit is contained in:
Louis-Guillaume DUBOIS 2015-04-22 21:38:54 +02:00
parent da3c812832
commit f59b124658
3 changed files with 107 additions and 13 deletions

View file

@ -112,6 +112,9 @@ add_executable(filtre src/filtre.cpp)
target_link_libraries(filtre ${catkin_LIBRARIES})
add_dependencies(filtre ${catkin_EXPORTED_TARGETS})
add_executable(random_pcl_publisher src/random_pcl_publisher.cpp)
target_link_libraries(random_pcl_publisher ${catkin_LIBRARIES})
add_dependencies(random_pcl_publisher ${catkin_EXPORTED_TARGETS})
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(hand_control_node hand_control_generate_messages_cpp)

View file

@ -1,36 +1,64 @@
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
class Callback {
public:
void operator()(const PointCloud::ConstPtr& msg)
void
operator()(const PointCloud::ConstPtr& msg)
{
ROS_INFO("PointCloud received");
// pour publier un shared_ptr (mieux)
PointCloud::Ptr pcl;
PointCloud::Ptr pcl(new PointCloud());
// copie du nuage de point
*pcl = *msg;
// TODO : ôter les mauvais points
// filtrage
double zmax(0.);
ros::param::getCached("zmax", zmax);
z_filtre.setFilterLimits(0.0, zmax);
z_filtre.setInputCloud(pcl);
z_filtre.filter(*pcl);
// publication
publisher.publish(pcl);
ROS_INFO("PointCloud published");
}
Callback(ros::Publisher& pub) : publisher(pub) {}
Callback(ros::Publisher& pub) : publisher(pub), z_filtre()
{
z_filtre.setFilterFieldName("z");
}
private:
ros::Publisher publisher;
pcl::PassThrough<pcl::PointXYZRGB> z_filtre;
};
int main(int argc, char** argv)
int
main(int argc, char** argv)
{
ros::init(argc, argv, "filtre");
ros::NodeHandle node;
// 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);
}
// initialisation
ros::Publisher publisher = node.advertise<PointCloud>("filtre_output", 1);
Callback callback(publisher);
ros::Subscriber subscriber = node.subscribe<PointCloud>("filtre_input", 1, callback);
// démarrage
ROS_INFO("node started");
ros::spin();
ROS_INFO("exit");

View file

@ -0,0 +1,63 @@
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
// for UniformGenerator
#include <pcl/common/random.h>
// for CloudGenerator
#include <pcl/common/generate.h>
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef pcl::common::UniformGenerator<float> UGenerator;
class Generator
{
public:
Generator(int l) : length(l), cgen()
{
UGenerator::Parameters params(-100, 100,-1);
cgen.setParameters(params);
}
PointCloud::Ptr
operator()()
{
PointCloud::Ptr pcl(new PointCloud());
cgen.fill(length, length, *pcl);
ROS_INFO("random cloud :");
for(int i = 0; i < pcl->points.size(); ++i)
{
ROS_INFO("\nx : %f\ny : %f\nz : %f\nr : %d\ng : %d\nb : %d",
pcl->points[i].x,
pcl->points[i].y,
pcl->points[i].z,
pcl->points[i].r,
pcl->points[i].g,
pcl->points[i].b);
}
return pcl;
}
private:
pcl::common::CloudGenerator<pcl::PointXYZRGB, UGenerator> cgen;
int length;
};
int
main(int argc, char** argv)
{
ros::init(argc, argv, "random_pcl_publisher");
ros::NodeHandle node;
ros::Publisher publisher = node.advertise<PointCloud>("random_output", 1);
Generator generator(5);
ros::Rate loop_rate(0.5);
ROS_INFO("node started");
while (ros::ok())
{
publisher.publish(generator());
ROS_INFO("random PointCloud published");
ros::spinOnce();
loop_rate.sleep();
}
ROS_INFO("exit");
return 0;
}