Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "jsk_pcl_ros_utils/cluster_point_indices_to_point_indices.h"
00036 #include <jsk_recognition_utils/pcl_conversion_util.h>
00037 #include <jsk_topic_tools/log_utils.h>
00038 #include <jsk_recognition_msgs/ClusterPointIndices.h>
00039 #include <boost/assign.hpp>
00040
00041 namespace jsk_pcl_ros_utils
00042 {
00043
00044 void ClusterPointIndicesToPointIndices::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047
00048 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00049 dynamic_reconfigure::Server<Config>::CallbackType f =
00050 boost::bind(&ClusterPointIndicesToPointIndices::configCallback, this, _1, _2);
00051 srv_->setCallback(f);
00052
00053 pub_ = advertise<PCLIndicesMsg>(*pnh_, "output", 1);
00054 onInitPostProcess();
00055 }
00056
00057 void ClusterPointIndicesToPointIndices::configCallback(
00058 Config &config, uint32_t level)
00059 {
00060 boost::mutex::scoped_lock lock(mutex_);
00061 index_ = config.index;
00062 }
00063
00064 void ClusterPointIndicesToPointIndices::subscribe()
00065 {
00066 sub_ = pnh_->subscribe("input", 1,
00067 &ClusterPointIndicesToPointIndices::convert,
00068 this);
00069 ros::V_string names = boost::assign::list_of("~input");
00070 jsk_topic_tools::warnNoRemap(names);
00071 }
00072
00073 void ClusterPointIndicesToPointIndices::unsubscribe()
00074 {
00075 sub_.shutdown();
00076 }
00077
00078 void ClusterPointIndicesToPointIndices::convert(
00079 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cluster_indices_msg)
00080 {
00081 vital_checker_->poke();
00082 PCLIndicesMsg indices_msg;
00083 indices_msg.header = cluster_indices_msg->header;
00084
00085 int cluster_size = cluster_indices_msg->cluster_indices.size();
00086 if (index_ < 0) {
00087
00088 return;
00089 } else if (index_ < cluster_size) {
00090 indices_msg.indices = cluster_indices_msg->cluster_indices[index_].indices;
00091 } else {
00092 NODELET_ERROR_THROTTLE(10, "Invalid ~index %d is specified for cluster size %d.", index_, cluster_size);
00093 }
00094 pub_.publish(indices_msg);
00095 }
00096
00097 }
00098
00099 #include <pluginlib/class_list_macros.h>
00100 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::ClusterPointIndicesToPointIndices, nodelet::Nodelet);