35 #define BOOST_PARAMETER_MAX_ARITY 7 45 DiagnosticNodelet::onInit();
49 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
64 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(
queue_size_);
69 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
86 const PCLIndicesMsg::ConstPtr& indices_msg,
90 cv::Mat mask_image = cv::Mat::zeros(height, width, CV_8UC1);
91 for (
size_t i = 0; i < indices_msg->indices.size(); i++) {
92 int index = indices_msg->indices[i];
93 if (index >= height * width || index < 0) {
94 ROS_ERROR(
"Input index is out of expected mask size.");
97 int width_index = index %
width;
98 int height_index = index /
width;
99 mask_image.at<uchar>(height_index, width_index) = 255;
108 const PCLIndicesMsg::ConstPtr& indices_msg)
115 const PCLIndicesMsg::ConstPtr& indices_msg,
116 const sensor_msgs::Image::ConstPtr& image_msg)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PointIndicesToMaskImage, nodelet::Nodelet)
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
virtual void unsubscribe()
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber sub_input_static_
virtual void convertAndPublish(const PCLIndicesMsg::ConstPtr &indices_msg, const int width, const int height)
message_filters::Subscriber< sensor_msgs::Image > sub_image_
message_filters::Subscriber< PCLIndicesMsg > sub_input_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void mask(const PCLIndicesMsg::ConstPtr &indices_msg)