39 #include <sensor_msgs/PointCloud2.h> 48 void callback(
const PointCloudT::ConstPtr& cloud)
50 PointCloudT::Ptr cloud_filtered = boost::make_shared<PointCloudT>();
51 cloud_filtered->header = cloud->header;
54 for (
size_t i = 0; i < cloud->size(); i++)
56 if (cloud->points[i].layer == 1)
58 cloud_filtered->points.push_back(cloud->points[i]);
65 int main(
int argc,
char **argv)
67 ros::init(argc, argv,
"sick_ldmrs_filter_layer");
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL void spin(Spinner &spinner)
void callback(const PointCloudT::ConstPtr &cloud)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
pcl::PointCloud< PointT > PointCloudT
sick_ldmrs_msgs::SICK_LDMRS_Point PointT
pcl::PointCloud< PointT > PointCloudT