43 DiagnosticNodelet::onInit();
48 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
49 dynamic_reconfigure::Server<Config>::CallbackType
f =
51 srv_->setCallback (f);
53 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"output", 1);
68 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(
queue_size_);
74 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
88 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cluster_msg,
89 const jsk_recognition_msgs::LabelArray::ConstPtr& label_msg)
91 jsk_recognition_msgs::ClusterPointIndices filtered_msg;
92 filtered_msg.header = cluster_msg->header;
94 for (
size_t i = 0; i < label_msg->labels.size(); i++) {
96 filtered_msg.cluster_indices.push_back(cluster_msg->cluster_indices[i]);
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
virtual void configCallback(Config &config, uint32_t level)
virtual void unsubscribe()
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void filter(const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &cluster_msg, const jsk_recognition_msgs::LabelArray::ConstPtr &label_msg)
message_filters::Subscriber< jsk_recognition_msgs::LabelArray > sub_labels_
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
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)
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
jsk_pcl_ros_utils::ClusterPointIndicesLabelFilterConfig Config
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::ClusterPointIndicesLabelFilter, nodelet::Nodelet)