diff --git a/hand_control/CMakeLists.txt b/hand_control/CMakeLists.txt index db4b01d..87e7100 100644 --- a/hand_control/CMakeLists.txt +++ b/hand_control/CMakeLists.txt @@ -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) diff --git a/hand_control/src/filtre.cpp b/hand_control/src/filtre.cpp index 8529eee..c7fa069 100644 --- a/hand_control/src/filtre.cpp +++ b/hand_control/src/filtre.cpp @@ -1,36 +1,64 @@ #include #include #include +#include typedef pcl::PointCloud PointCloud; class Callback { public: - void operator()(const PointCloud::ConstPtr& msg) - { - ROS_INFO("PointCloud received"); - // pour publier un shared_ptr (mieux) - PointCloud::Ptr pcl; - // copie du nuage de point - *pcl = *msg; - // TODO : ôter les mauvais points - publisher.publish(pcl); - ROS_INFO("PointCloud published"); - } + void + operator()(const PointCloud::ConstPtr& msg) + { + ROS_INFO("PointCloud received"); + // pour publier un shared_ptr (mieux) + PointCloud::Ptr pcl(new PointCloud()); + // copie du nuage de point + *pcl = *msg; + // 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 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("filtre_output", 1); Callback callback(publisher); ros::Subscriber subscriber = node.subscribe("filtre_input", 1, callback); + + // démarrage ROS_INFO("node started"); ros::spin(); ROS_INFO("exit"); diff --git a/hand_control/src/random_pcl_publisher.cpp b/hand_control/src/random_pcl_publisher.cpp index e69de29..88089f7 100644 --- a/hand_control/src/random_pcl_publisher.cpp +++ b/hand_control/src/random_pcl_publisher.cpp @@ -0,0 +1,63 @@ +#include +#include +#include +// for UniformGenerator +#include +// for CloudGenerator +#include + +typedef pcl::PointCloud PointCloud; +typedef pcl::common::UniformGenerator 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 cgen; + int length; +}; + +int +main(int argc, char** argv) +{ + ros::init(argc, argv, "random_pcl_publisher"); + ros::NodeHandle node; + ros::Publisher publisher = node.advertise("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; +}