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 #define BOOST_PARAMETER_MAX_ARITY 7
00036
00037 #include "jsk_pcl_ros_utils/bounding_box_array_to_bounding_box.h"
00038 #include <jsk_recognition_utils/pcl_conversion_util.h>
00039 #include <jsk_recognition_msgs/BoundingBox.h>
00040
00041 namespace jsk_pcl_ros_utils
00042 {
00043
00044 void BoundingBoxArrayToBoundingBox::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047
00048
00049 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00050 dynamic_reconfigure::Server<Config>::CallbackType f =
00051 boost::bind(&BoundingBoxArrayToBoundingBox::configCallback, this, _1, _2);
00052 srv_->setCallback(f);
00053
00054 pub_ = advertise<jsk_recognition_msgs::BoundingBox>(*pnh_, "output", 1);
00055 onInitPostProcess();
00056 }
00057
00058 void BoundingBoxArrayToBoundingBox::configCallback(
00059 Config &config, uint32_t level)
00060 {
00061 boost::mutex::scoped_lock lock(mutex_);
00062 index_ = config.index;
00063 }
00064
00065 void BoundingBoxArrayToBoundingBox::subscribe()
00066 {
00067 sub_ = pnh_->subscribe("input", 1, &BoundingBoxArrayToBoundingBox::convert, this);
00068 }
00069
00070 void BoundingBoxArrayToBoundingBox::unsubscribe()
00071 {
00072 sub_.shutdown();
00073 }
00074
00075 void BoundingBoxArrayToBoundingBox::convert(
00076 const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& bbox_array_msg)
00077 {
00078 vital_checker_->poke();
00079
00080 jsk_recognition_msgs::BoundingBox bbox_msg;
00081 bbox_msg.header = bbox_array_msg->header;
00082
00083 int array_size = bbox_array_msg->boxes.size();
00084 if (index_ < 0) {
00085 return;
00086 } else if (index_ < array_size) {
00087 bbox_msg = bbox_array_msg->boxes[index_];
00088 } else {
00089 NODELET_ERROR_THROTTLE(10, "Invalid ~index %d is specified for array size %d.", index_, array_size);
00090 }
00091
00092 pub_.publish(bbox_msg);
00093 }
00094
00095 }
00096 #include <pluginlib/class_list_macros.h>
00097 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::BoundingBoxArrayToBoundingBox, nodelet::Nodelet);