38 #include <pcl/point_cloud.h> 39 #include <pcl/point_types.h> 46 DiagnosticNodelet::onInit();
47 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"output", 1);
48 pnh_->param<
bool>(
"skip_nan",
skip_nan_,
false);
64 const sensor_msgs::PointCloud2::ConstPtr& msg)
67 pcl::PointCloud<pcl::PointXYZRGB>
cloud;
69 int point_num = msg->width * msg->height;
70 pcl_msgs::PointIndices indices;
71 jsk_recognition_msgs::ClusterPointIndices cluster_indices;
72 for (
int i = 0; i < point_num; i++) {
74 (std::isnan(cloud.points[i].x) || std::isnan(cloud.points[i].y) || std::isnan(cloud.points[i].z)))
78 indices.indices.push_back(i);
80 indices.header = msg->header;
81 cluster_indices.header = msg->header;
82 cluster_indices.cluster_indices.push_back(indices);
void publish(const boost::shared_ptr< M > &message) const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void convert(const sensor_msgs::PointCloud2::ConstPtr &msg)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PointCloudToClusterPointIndices, nodelet::Nodelet)
virtual void unsubscribe()