Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros_utils/pointcloud_to_point_indices.h"
00037 #include <jsk_recognition_utils/pcl_conversion_util.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <sensor_msgs/image_encodings.h>
00040
00041 namespace jsk_pcl_ros_utils
00042 {
00043 void PointCloudToPointIndices::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 pub_ = advertise<PCLIndicesMsg>(*pnh_, "output", 1);
00047 onInitPostProcess();
00048 }
00049
00050 void PointCloudToPointIndices::subscribe()
00051 {
00052 sub_ = pnh_->subscribe("input", 1, &PointCloudToPointIndices::convert, this);
00053 }
00054
00055 void PointCloudToPointIndices::unsubscribe()
00056 {
00057 sub_.shutdown();
00058 }
00059
00060 void PointCloudToPointIndices::updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
00061 {
00062 if (vital_checker_->isAlive())
00063 {
00064 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "PointCloudToPointIndices running");
00065 }
00066 else
00067 {
00068 jsk_topic_tools::addDiagnosticErrorSummary("PointCloudToPointIndices", vital_checker_, stat);
00069 }
00070 }
00071
00072 void PointCloudToPointIndices::convert(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00073 {
00074 vital_checker_->poke();
00075 pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
00076 pcl::fromROSMsg(*cloud_msg, *pc);
00077 PCLIndicesMsg indices_msg;
00078 for (size_t i = 0; i < pc->points.size(); i++)
00079 {
00080 if (!isnan(pc->points[i].x) && !isnan(pc->points[i].y) && !isnan(pc->points[i].z))
00081 {
00082 indices_msg.indices.push_back(i);
00083 }
00084 }
00085 indices_msg.header = cloud_msg->header;
00086 pub_.publish(indices_msg);
00087 }
00088 }
00089
00090 #include <pluginlib/class_list_macros.h>
00091 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PointCloudToPointIndices, nodelet::Nodelet);