35 #define BOOST_PARAMETER_MAX_ARITY 7 45 DiagnosticNodelet::onInit();
50 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"output/all_indices", 1);
52 pub_ = advertise<PCLIndicesMsg>(*
pnh_,
"output", 1);
70 const sensor_msgs::Image::ConstPtr& image_msg)
75 cv::Mat image = cv_ptr->image;
77 jsk_recognition_msgs::ClusterPointIndices cluster_msg;
78 cluster_msg.header = image_msg->header;
79 cluster_msg.cluster_indices.resize(image.channels());
80 for (
size_t c = 0;
c < image.channels(); ++
c) {
82 indices_msg.header = image_msg->header;
83 for (
size_t j = 0; j < image.rows; j++) {
84 for (
size_t i = 0; i < image.cols; i++) {
85 if (image.data[j * image.step + i * image.elemSize() +
c] > 127) {
86 indices_msg.indices.push_back(j * image.cols + i);
94 NODELET_ERROR(
"target_channel_ is %d, but image has %d channels",
99 indices_msg.header = image_msg->header;
100 for (
size_t j = 0; j < image.rows; j++) {
101 for (
size_t i = 0; i < image.cols; i++) {
102 if (image.data[j * image.step + i * image.elemSize() +
target_channel_] > 127) {
103 indices_msg.indices.push_back(j * image.cols + i);
112 cv::Mat image = cv_ptr->image;
114 indices_msg.header = image_msg->header;
115 for (
size_t j = 0; j < image.rows; j++) {
116 for (
size_t i = 0; i < image.cols; i++) {
117 if (image.at<uchar>(j, i) > 127) {
118 indices_msg.indices.push_back(j * image.cols + i);
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
const std::string TYPE_8UC1
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::MaskImageToPointIndices, nodelet::Nodelet)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
pcl::PointIndices PCLIndicesMsg
virtual void unsubscribe()
virtual void indices(const sensor_msgs::Image::ConstPtr &image_msg)