35 #define BOOST_PARAMETER_MAX_ARITY 7 45 DiagnosticNodelet::onInit();
46 pub_ = advertise<PCLIndicesMsg>(*
pnh_,
"output", 1);
63 pcl::PointCloud<pcl::PointXYZ>::Ptr pc(
new pcl::PointCloud<pcl::PointXYZ>);
66 for (
size_t i = 0; i < pc->points.size(); i++)
68 if (!isnan(pc->points[i].x) && !isnan(pc->points[i].y) && !isnan(pc->points[i].z))
70 indices_msg.indices.push_back(i);
73 indices_msg.header = cloud_msg->header;
void publish(const boost::shared_ptr< M > &message) const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void unsubscribe()
virtual void convert(const sensor_msgs::PointCloud2::ConstPtr &point_msg)
pcl::PointIndices PCLIndicesMsg
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PointCloudToPointIndices, nodelet::Nodelet)