00001 #include <ros/ros.h>
00002 #include <iostream>
00003 #include <pcl_ros/point_cloud.h>
00004 #include <pcl/point_types.h>
00005 #include "pcl/io/pcd_io.h"
00006
00007 #include "ClearpathPointCloudTools.h"
00008
00009 typedef pcl::PointXYZRGB PointType;
00010 typedef pcl::PointCloud<PointType> PointCloud;
00011
00012 ros::Publisher pub;
00013
00014 double dist = 0.0;
00015
00016 void callback(const PointCloud::ConstPtr& cloud)
00017 {
00018 PointCloud result;
00019 ClearpathPointCloudTools::PassthroughPointCloudImage((PointCloud*)&(*cloud), &result, dist);
00020 pub.publish (result);
00021 }
00022
00023 int main(int argc, char** argv)
00024 {
00025 ros::init (argc, argv, "passthrough_default");
00026
00027 ros::NodeHandle nh = ros::NodeHandle(ros::this_node::getName());
00028 std::string in, out;
00029 nh.param<std::string>("in", in, "passthrough_in");
00030 nh.param<std::string>("out", out, "passthrough_out");
00031 nh.param<double>("dist", dist, 1.0);
00032
00033 ros::NodeHandle nx;
00034 ros::Subscriber sub = nx.subscribe<PointCloud>(in, 1, callback);
00035 pub = nx.advertise<PointCloud> (out, 1);
00036
00037 ros::spin();
00038
00039 return (0);
00040 }
00041
00042
00043
