38 #include <jsk_recognition_msgs/ClusterPointIndices.h> 39 #include <boost/assign.hpp> 46 DiagnosticNodelet::onInit();
48 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
49 dynamic_reconfigure::Server<Config>::CallbackType
f =
53 pub_ = advertise<PCLIndicesMsg>(*
pnh_,
"output", 1);
58 Config &config, uint32_t level)
79 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cluster_indices_msg)
83 indices_msg.header = cluster_indices_msg->header;
85 int cluster_size = cluster_indices_msg->cluster_indices.size();
87 for(
int i = 0;i < cluster_size; ++i) {
88 indices_msg.indices.insert(indices_msg.indices.end(),
89 cluster_indices_msg->cluster_indices[i].indices.begin(),
90 cluster_indices_msg->cluster_indices[i].indices.end());
92 }
else if (
index_ < cluster_size) {
93 indices_msg.indices = cluster_indices_msg->cluster_indices[
index_].indices;
void publish(const boost::shared_ptr< M > &message) const
#define NODELET_ERROR_THROTTLE(rate,...)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void convert(const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &cluster_indices_msg)
std::vector< std::string > V_string
jsk_pcl_ros_utils::ClusterPointIndicesToPointIndicesConfig Config
virtual void unsubscribe()
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::ClusterPointIndicesToPointIndices, nodelet::Nodelet)
virtual void configCallback(Config &config, uint32_t level)
pcl::PointIndices PCLIndicesMsg