36 #define BOOST_PARAMETER_MAX_ARITY 7 47 DiagnosticNodelet::onInit();
49 pub_ = advertise<PCLIndicesMsg>(
51 DiagnosticNodelet::onInitPostProcess();
71 const sensor_msgs::CameraInfo::ConstPtr& info_ms)
78 const sensor_msgs::Image::ConstPtr& mask_msg)
91 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
97 pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud 98 (
new pcl::PointCloud<pcl::PointXYZ>);
101 indices.header = cloud_msg->header;
102 for (
size_t i = 0; i < cloud->points.size(); i++) {
103 pcl::PointXYZ
p = cloud->points[i];
109 indices.indices.push_back(i);
virtual void infoCalback(const sensor_msgs::CameraInfo::ConstPtr &info_ms)
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber sub_cloud_
sensor_msgs::CameraInfo::ConstPtr camera_info_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void unsubscribe()
ros::Subscriber sub_image_
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::MaskImageFilter, nodelet::Nodelet)
virtual void filter(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
ros::Subscriber sub_info_
virtual void imageCalback(const sensor_msgs::Image::ConstPtr &mask_msg)
cv::Point2d project3dToPixel(const cv::Point3d &xyz) const
pcl::PointIndices PCLIndicesMsg