attention_clipper_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/attention_clipper.h"
00037 #include <eigen_conversions/eigen_msg.h>
00038 #include <tf_conversions/tf_eigen.h>
00039 #include <opencv2/opencv.hpp>
00040 #include <cv_bridge/cv_bridge.h>
00041 #include <sensor_msgs/image_encodings.h>
00042 #include <pcl_ros/transforms.h>
00043 #include "jsk_recognition_utils/pcl_conversion_util.h"
00044 #include <jsk_topic_tools/rosparam_utils.h>
00045 #include "jsk_recognition_utils/pcl_ros_util.h"
00046 #include "jsk_recognition_utils/pcl_util.h"
00047 #include <algorithm>
00048 #include <set>
00049 namespace jsk_pcl_ros
00050 {
00051   void AttentionClipper::onInit()
00052   {
00053     DiagnosticNodelet::onInit();
00054     tf_listener_ = TfListenerSingleton::getInstance();
00055     pnh_->param("negative", negative_, false);
00056     pnh_->param("use_multiple_attention", use_multiple_attention_, false);
00057     if (!use_multiple_attention_) {
00058       Eigen::Affine3f pose = Eigen::Affine3f::Identity();
00059       std::vector<double> initial_pos;
00060       if (jsk_topic_tools::readVectorParameter(*pnh_,
00061                                                "initial_pos",
00062                                                initial_pos))
00063       {
00064         pose.translation() = Eigen::Vector3f(initial_pos[0],
00065                                               initial_pos[1],
00066                                               initial_pos[2]);
00067       }
00068 
00069       std::vector<double> initial_rot;
00070       if (jsk_topic_tools::readVectorParameter(*pnh_,
00071                                                "initial_rot",
00072                                                initial_rot))
00073       {
00074         pose = pose
00075           * Eigen::AngleAxisf(initial_rot[0],
00076                               Eigen::Vector3f::UnitX())
00077           * Eigen::AngleAxisf(initial_rot[1],
00078                               Eigen::Vector3f::UnitY())
00079           * Eigen::AngleAxisf(initial_rot[2],
00080                               Eigen::Vector3f::UnitZ());
00081       }
00082 
00083       // parameter
00084       // backward compatibility
00085       double dimension_x, dimension_y, dimension_z;
00086       pnh_->param("dimension_x", dimension_x, 0.1);
00087       pnh_->param("dimension_y", dimension_y, 0.1);
00088       pnh_->param("dimension_z", dimension_z, 0.1);
00089       dimensions_.push_back(Eigen::Vector3f(dimension_x,
00090                                             dimension_y,
00091                                             dimension_z));
00092       std::string frame_id;
00093       pnh_->param("frame_id", frame_id, std::string("base_link"));
00094       frame_id_list_.push_back(frame_id);
00095       pose_list_.push_back(pose);
00096     }
00097     else {                      // multiple interst
00098       // ~initial_pos_list
00099       //   -> [[0, 0, 0], ...]
00100       std::vector<std::vector<double> > initial_pos_list;
00101       std::vector<std::vector<double> > initial_rot_list;
00102       std::vector<std::vector<double> > dimensions;
00103       jsk_topic_tools::readVectorParameter(*pnh_, "initial_pos_list",
00104                                            initial_pos_list);
00105       jsk_topic_tools::readVectorParameter(*pnh_, "initial_rot_list",
00106                                            initial_rot_list);
00107       jsk_topic_tools::readVectorParameter(*pnh_, "dimensions", dimensions);
00108       jsk_topic_tools::readVectorParameter(*pnh_, "prefixes", prefixes_);
00109       if (initial_pos_list.size() != 0) {
00110         initializePoseList(initial_pos_list.size());
00111         for (size_t i = 0; i < initial_pos_list.size(); i++) {
00112           if (initial_pos_list[i].size() != 3) {
00113             NODELET_FATAL("element of ~initial_pos_list should be [x, y, z]");
00114             return;
00115           }
00116           pose_list_[i].translation() = Eigen::Vector3f(initial_pos_list[i][0],
00117                                                         initial_pos_list[i][1],
00118                                                         initial_pos_list[i][2]);
00119         }
00120       }
00121       // ~initial_rot_list
00122       //   -> [[0, 0, 0], ...]
00123       if (initial_rot_list.size() != 0) {
00124         // error check
00125         if (initial_pos_list.size() != 0 &&
00126             initial_rot_list.size() != initial_pos_list.size()) {
00127           NODELET_FATAL(
00128             "the size of ~initial_pos_list and ~initial_rot_list are different");
00129           return;
00130         }
00131         if (initial_pos_list.size() == 0) {
00132           initializePoseList(initial_rot_list.size());
00133         }
00134         for (size_t i = 0; i < initial_rot_list.size(); i++) {
00135           if (initial_rot_list[i].size() != 3) {
00136             NODELET_FATAL("element of ~initial_rot_list should be [rx, ry, rz]");
00137             return;
00138           }
00139           pose_list_[i] = pose_list_[i]
00140             * Eigen::AngleAxisf(initial_rot_list[i][0],
00141                                 Eigen::Vector3f::UnitX())
00142             * Eigen::AngleAxisf(initial_rot_list[i][1],
00143                                 Eigen::Vector3f::UnitY())
00144             * Eigen::AngleAxisf(initial_rot_list[i][2],
00145                                 Eigen::Vector3f::UnitZ());
00146         }
00147       }
00148       // ~dimensions
00149       //   -> [[x, y, z], [x, y, z] ...]
00150       if (dimensions.size() != 0) {
00151         // error check
00152         if (pose_list_.size() != 0 &&
00153             pose_list_.size() != dimensions.size()) {
00154           NODELET_FATAL(
00155             "the size of ~dimensions and ~initial_pos_list or ~initial_rot_list are different");
00156           return;
00157         }
00158         if (pose_list_.size() == 0) {
00159           initializePoseList(dimensions.size());
00160         }
00161         for (size_t i = 0; i < dimensions.size(); i++) {
00162           dimensions_.push_back(Eigen::Vector3f(dimensions[i][0],
00163                                                 dimensions[i][1],
00164                                                 dimensions[i][2]));
00165         }
00166       }
00167 
00168       // ~prefixes
00169       //   -> [left_hand, right_hand ...]
00170       if (prefixes_.size() != 0) {
00171         // error check
00172         if (prefixes_.size() != dimensions.size()) {
00173           NODELET_FATAL(
00174             "the size of ~prefixes and ~dimensions are different");
00175           return;
00176         }
00177         for(int i = 0; i < prefixes_.size(); i++){
00178           multiple_pub_indices_.push_back(advertise<PCLIndicesMsg>(*pnh_, prefixes_[i]+std::string("/point_indices"), 1));
00179         }
00180       }
00181 
00182       jsk_topic_tools::readVectorParameter(*pnh_, "frame_id_list", frame_id_list_);
00183     }
00184     pub_camera_info_ = advertise<sensor_msgs::CameraInfo>(*pnh_, "output", 1);
00185     pub_bounding_box_array_
00186       = advertise<jsk_recognition_msgs::BoundingBoxArray>(*pnh_, "output/box_array", 1);
00187     pub_mask_ = advertise<sensor_msgs::Image>(*pnh_, "output/mask", 1);
00188     pub_indices_ = advertise<PCLIndicesMsg>(*pnh_, "output/point_indices", 1);
00189     pub_cluster_indices_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output/cluster_point_indices", 1);
00190 
00191     onInitPostProcess();
00192   }
00193 
00194   void AttentionClipper::initializePoseList(size_t num)
00195   {
00196     pose_list_.resize(num);
00197     for (size_t i = 0; i < pose_list_.size(); i++) {
00198       pose_list_[i].setIdentity();
00199     }
00200   }
00201 
00202   void AttentionClipper::subscribe()
00203   {
00204     sub_ = pnh_->subscribe("input", 1, &AttentionClipper::clip, this);
00205     sub_points_ = pnh_->subscribe("input/points", 1,
00206                                   &AttentionClipper::clipPointcloud, this);
00207     if (!use_multiple_attention_) {
00208       sub_pose_ = pnh_->subscribe("input/pose",
00209                                   1, &AttentionClipper::poseCallback, this);
00210       sub_box_ = pnh_->subscribe("input/box",
00211                                  1, &AttentionClipper::boxCallback, this);
00212     }
00213     else {
00214       sub_pose_ = pnh_->subscribe("input/pose_array",
00215                                   1, &AttentionClipper::poseArrayCallback, this);
00216       sub_box_ = pnh_->subscribe("input/box_array",
00217                                  1, &AttentionClipper::boxArrayCallback, this);
00218     }
00219   }
00220 
00221   void AttentionClipper::unsubscribe()
00222   {
00223     sub_.shutdown();
00224     sub_points_.shutdown();
00225     sub_pose_.shutdown();
00226     sub_box_.shutdown();
00227   }
00228 
00229   jsk_recognition_utils::Vertices AttentionClipper::cubeVertices(Eigen::Vector3f& dimension)
00230   {
00231     jsk_recognition_utils::Vertices nonoffsetted_vertices;
00232     nonoffsetted_vertices.push_back(
00233       Eigen::Vector3f(-dimension[0]/2, -dimension[1]/2, -dimension[2]/2));
00234     nonoffsetted_vertices.push_back(
00235       Eigen::Vector3f(-dimension[0]/2, -dimension[1]/2, dimension[2]/2));
00236     nonoffsetted_vertices.push_back(
00237       Eigen::Vector3f(-dimension[0]/2, dimension[1]/2, -dimension[2]/2));
00238     nonoffsetted_vertices.push_back(
00239       Eigen::Vector3f(-dimension[0]/2, dimension[1]/2, dimension[2]/2));
00240     nonoffsetted_vertices.push_back(
00241       Eigen::Vector3f(dimension[0]/2, -dimension[1]/2, -dimension[2]/2));
00242     nonoffsetted_vertices.push_back(
00243       Eigen::Vector3f(dimension[0]/2, -dimension[1]/2, dimension[2]/2));
00244     nonoffsetted_vertices.push_back(
00245       Eigen::Vector3f(dimension[0]/2, dimension[1]/2, -dimension[2]/2));
00246     nonoffsetted_vertices.push_back(
00247       Eigen::Vector3f(dimension[0]/2, dimension[1]/2, dimension[2]/2));
00248     return nonoffsetted_vertices;
00249   }
00250 
00251   // callback only for one interaction
00252   void AttentionClipper::poseCallback(
00253     const geometry_msgs::PoseStamped::ConstPtr& pose)
00254   {
00255     boost::mutex::scoped_lock lock(mutex_);
00256     frame_id_list_[0] = pose->header.frame_id;
00257     tf::poseMsgToEigen(pose->pose, pose_list_[0]);
00258   }
00259 
00260   void AttentionClipper::boxCallback(
00261     const jsk_recognition_msgs::BoundingBox::ConstPtr& box)
00262   {
00263     boost::mutex::scoped_lock lock(mutex_);
00264     dimensions_[0][0] = box->dimensions.x;
00265     dimensions_[0][1] = box->dimensions.y;
00266     dimensions_[0][2] = box->dimensions.z;
00267     frame_id_list_[0] = box->header.frame_id;
00268     tf::poseMsgToEigen(box->pose, pose_list_[0]);
00269   }
00270 
00271   // callback for multiple interactions
00272   void AttentionClipper::poseArrayCallback(
00273     const geometry_msgs::PoseArray::ConstPtr& pose_array)
00274   {
00275     boost::mutex::scoped_lock lock(mutex_);
00276     // resize
00277     initializePoseList(pose_array->poses.size());
00278     frame_id_list_.resize(pose_array->poses.size());
00279     std::fill(frame_id_list_.begin(), frame_id_list_.end(),
00280               pose_array->header.frame_id);
00281     for (size_t i = 0; i < pose_list_.size(); i++) {
00282       tf::poseMsgToEigen(pose_array->poses[i], pose_list_[i]);
00283     }
00284   }
00285 
00286   void AttentionClipper::boxArrayCallback(
00287     const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& box_array)
00288   {
00289     boost::mutex::scoped_lock lock(mutex_);
00290     initializePoseList(box_array->boxes.size());
00291     frame_id_list_.resize(box_array->boxes.size());
00292     dimensions_.resize(box_array->boxes.size());
00293     for (size_t i = 0; i < pose_list_.size(); i++) {
00294       tf::poseMsgToEigen(box_array->boxes[i].pose, pose_list_[i]);
00295       frame_id_list_[i] = box_array->boxes[i].header.frame_id;
00296       dimensions_[i] = Eigen::Vector3f(box_array->boxes[i].dimensions.x,
00297                                        box_array->boxes[i].dimensions.y,
00298                                        box_array->boxes[i].dimensions.z);
00299     }
00300   }
00301 
00302   void AttentionClipper::publishBoundingBox(
00303     const std_msgs::Header& header)
00304   {
00305     jsk_recognition_msgs::BoundingBoxArray box_array;
00306     box_array.header.frame_id = header.frame_id;
00307     box_array.header.stamp = header.stamp;
00308     for (size_t i = 0; i < pose_list_.size(); i++) {
00309       jsk_recognition_msgs::BoundingBox box;
00310       box.header = header;
00311       tf::poseEigenToMsg(transformed_pose_list_[i], box.pose);
00312       jsk_recognition_utils::pointFromVectorToXYZ(dimensions_[i], box.dimensions);
00313       box_array.boxes.push_back(box);
00314     }
00315     pub_bounding_box_array_.publish(box_array);
00316   }
00317 
00318   void AttentionClipper::computeROI(
00319     const sensor_msgs::CameraInfo::ConstPtr& msg,
00320     std::vector<cv::Point2d>& points,
00321     cv::Mat& mask)
00322   {
00323     double min_u, min_v, max_u, max_v;
00324     min_u = msg->width;
00325     min_v = msg->height;
00326     max_u = max_v = 0;
00327     for (size_t i = 0; i < points.size(); i++) {
00328       cv::Point2d uv(points[i]);
00329       // check limit
00330       if (uv.x < 0) {
00331         uv.x = 0;
00332       }
00333       if (uv.y < 0) {
00334         uv.y = 0;
00335       }
00336       if (uv.x > msg->width) {
00337         uv.x = msg->width;
00338       }
00339 
00340       if (uv.y > msg->height) {
00341         uv.y = msg->height;
00342       }
00343       if (min_u > uv.x) {
00344         min_u = uv.x;
00345       }
00346       if (max_u < uv.x) {
00347         max_u = uv.x;
00348       }
00349       if (min_v > uv.y) {
00350         min_v = uv.y;
00351       }
00352 
00353       if (max_v < uv.y) {
00354         max_v = uv.y;
00355       }
00356     }
00357     // now we have min/max of u/v
00358     cv::Rect raw_roi(min_u, min_v, (max_u - min_u), (max_v - min_v));
00359     cv::Rect original(0, 0, msg->width, msg->height);
00360     cv::Rect roi_region = raw_roi & original;
00361     sensor_msgs::CameraInfo roi(*msg);
00362     roi.roi.x_offset = roi_region.x;
00363     roi.roi.y_offset = roi_region.y;
00364     roi.roi.width = roi_region.width;
00365     roi.roi.height = roi_region.height;
00366     roi.roi.do_rectify = true;
00367     pub_camera_info_.publish(roi);
00368     // mask computation
00369     mask = cv::Mat::zeros(msg->height, msg->width, CV_8UC1);
00370     cv::Size roi_size(roi_region.width, roi_region.height);
00371     cv::Rect roi_rect(cv::Point(roi_region.x, roi_region.y), roi_size);
00372     const cv::Scalar white(255);
00373     cv::rectangle(mask, roi_rect, white, CV_FILLED);
00374   }
00375 
00376   void AttentionClipper::clipPointcloud(
00377     const sensor_msgs::PointCloud2::ConstPtr& msg)
00378   {
00379     boost::mutex::scoped_lock lock(mutex_);
00380     NODELET_DEBUG("clipPointcloud");
00381     vital_checker_->poke();
00382     try {
00383       // 1. transform pointcloud
00384       // 2. crop by boundingbox
00385       // 3. publish indices
00386       pcl::PointIndices::Ptr all_indices (new pcl::PointIndices);
00387       jsk_recognition_msgs::ClusterPointIndices cluster_indices_msg;
00388       std::map<std::string, tf::StampedTransform> transforms;
00389       transformed_pose_list_.clear();
00390       for (size_t i = 0; i < pose_list_.size(); i++) {
00391         std::string frame_id = frame_id_list_[i];
00392         // check transform cache
00393         if (transforms.find(frame_id) == transforms.end()) {
00394           tf::StampedTransform new_transform = lookupTransformWithDuration(
00395             /*listener=*/tf_listener_,
00396             /*to_frame=*/frame_id,                // box origin
00397             /*from_frame=*/msg->header.frame_id,  // sensor origin
00398             /*time=*/msg->header.stamp,
00399             /*duration=*/ros::Duration(1.0));
00400           transforms[frame_id] = new_transform; // sensor to box
00401         }
00402         tf::StampedTransform tf_transform = transforms[frame_id];
00403         Eigen::Affine3f transform;
00404         tf::transformTFToEigen(tf_transform, transform);
00405         pcl::PointCloud<pcl::PointXYZ>::Ptr
00406           cloud(new pcl::PointCloud<pcl::PointXYZ>);
00407         pcl::fromROSMsg(*msg, *cloud);
00408 
00409         Eigen::Affine3f transformed_box_pose = transform * pose_list_[i];
00410         transformed_pose_list_.push_back(transformed_box_pose);
00411 
00412         pcl::PointIndices::Ptr indices (new pcl::PointIndices);
00413         jsk_recognition_msgs::BoundingBox bbox_msg;
00414         bbox_msg.header.frame_id = cloud->header.frame_id;
00415         tf::poseEigenToMsg(transformed_box_pose, bbox_msg.pose);
00416         bbox_msg.dimensions.x = dimensions_[i][0];
00417         bbox_msg.dimensions.y = dimensions_[i][1];
00418         bbox_msg.dimensions.z = dimensions_[i][2];
00419         jsk_recognition_utils::cropPointCloud<pcl::PointXYZ>(cloud, bbox_msg, &(indices->indices));
00420 
00421         // indices->indices may include NaN and inf points
00422         // https://github.com/jsk-ros-pkg/jsk_recognition/issues/888
00423         pcl::PointIndices non_nan_indices;
00424         for (size_t j = 0; j < indices->indices.size(); j++) {
00425           pcl::PointXYZ p = cloud->points[indices->indices[j]];
00426           if (pcl_isfinite(p.x) && pcl_isfinite(p.y) && pcl_isfinite(p.z)) {
00427             non_nan_indices.indices.push_back(indices->indices[j]);
00428           }
00429         }
00430         PCLIndicesMsg indices_msg;
00431         pcl_conversions::fromPCL(non_nan_indices, indices_msg);
00432         cluster_indices_msg.cluster_indices.push_back(indices_msg);
00433         if(prefixes_.size()){
00434           indices_msg.header = msg->header;
00435           multiple_pub_indices_[i].publish(indices_msg);
00436         }
00437 
00438         all_indices = jsk_recognition_utils::addIndices(*all_indices, non_nan_indices);
00439       }
00440       if (negative_) {
00441         // Publish indices which is NOT inside of box.
00442         pcl::PointIndices::Ptr tmp_indices (new pcl::PointIndices);
00443         std::set<int> positive_indices(all_indices->indices.begin(), all_indices->indices.end());
00444         for (size_t i = 0; i < msg->width * msg->height; i++) {
00445           if (positive_indices.find(i) == positive_indices.end()) {
00446             tmp_indices->indices.push_back(i);
00447           }
00448         }
00449         all_indices = tmp_indices;
00450       }
00451       PCLIndicesMsg indices_msg;
00452       pcl_conversions::fromPCL(*all_indices, indices_msg);
00453       cluster_indices_msg.header = indices_msg.header = msg->header;
00454       pub_indices_.publish(indices_msg);
00455       pub_cluster_indices_.publish(cluster_indices_msg);
00456       publishBoundingBox(msg->header);
00457     }
00458     catch (std::runtime_error &e) {
00459       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00460     } 
00461   }
00462 
00463   void AttentionClipper::clip(const sensor_msgs::CameraInfo::ConstPtr& msg)
00464   {
00465     boost::mutex::scoped_lock lock(mutex_);
00466     vital_checker_->poke();
00467     // resolve tf for all interest
00468     try {
00469       image_geometry::PinholeCameraModel model;
00470       cv::Mat all_mask_image = cv::Mat::zeros(msg->height, msg->width, CV_8UC1);
00471       bool model_success_p = model.fromCameraInfo(msg);
00472       if (!model_success_p) {
00473         ROS_ERROR("failed to create camera model");
00474         return;
00475       }
00476       for (size_t i = 0; i < pose_list_.size(); i++) {
00477         std::string frame_id = frame_id_list_[i];
00478         tf_listener_->waitForTransform(frame_id,
00479                                        msg->header.frame_id,
00480                                        msg->header.stamp,
00481                                        ros::Duration(1.0));
00482         Eigen::Affine3f offset = pose_list_[i];
00483         if (tf_listener_->canTransform(msg->header.frame_id,
00484                                        frame_id,
00485                                        msg->header.stamp)) {
00486           tf::StampedTransform transform; // header -> frame_id_
00487           tf_listener_->lookupTransform(frame_id, msg->header.frame_id,
00488                                         msg->header.stamp, transform);
00489           Eigen::Affine3f eigen_transform;
00490           tf::transformTFToEigen(transform, eigen_transform);
00491           jsk_recognition_utils::Vertices original_vertices = cubeVertices(dimensions_[i]);
00492           jsk_recognition_utils::Vertices vertices;
00493           for (size_t i = 0; i < original_vertices.size(); i++) {
00494             vertices.push_back(eigen_transform.inverse()
00495                                * (offset * original_vertices[i]));
00496           }
00497           std::vector<cv::Point2d> local_points;
00498           for (size_t i = 0; i < vertices.size(); i++) {
00499             cv::Point3d p(vertices[i][0], vertices[i][1], vertices[i][2]);
00500             cv::Point2d uv = model.project3dToPixel(p);
00501             local_points.push_back(uv);
00502           }
00503           cv::Mat mask_image;
00504           computeROI(msg, local_points, mask_image);
00505           all_mask_image = all_mask_image | mask_image;
00506         }
00507       }
00508       // publish
00509       cv_bridge::CvImage mask_bridge(msg->header,
00510                                      sensor_msgs::image_encodings::MONO8,
00511                                      all_mask_image);
00512       pub_mask_.publish(mask_bridge.toImageMsg());
00513       //publishBoundingBox(msg->header);
00514     }
00515     catch (std::runtime_error &e) {
00516       NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
00517     } 
00518   }
00519 
00520   void AttentionClipper::updateDiagnostic(
00521     diagnostic_updater::DiagnosticStatusWrapper &stat)
00522   {
00523     if (vital_checker_->isAlive()) {
00524       stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00525                    "AttentionClipper running");
00526     }
00527     else {
00528       jsk_topic_tools::addDiagnosticErrorSummary(
00529         "AttentionClipper", vital_checker_, stat);
00530     }
00531   }
00532 
00533 }
00534 
00535 #include <pluginlib/class_list_macros.h>
00536 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::AttentionClipper, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49