37 #include <boost/assign.hpp> 39 #include <opencv2/opencv.hpp> 47 DiagnosticNodelet::onInit();
48 pub_ = advertise<jsk_recognition_msgs::RectArray>(*
pnh_,
"output", 1);
65 const sensor_msgs::Image::ConstPtr& mask_msg)
68 std::vector<cv::Point> indices;
71 cv::Mat mask = cv_ptr->image;
72 for (
size_t j = 0; j < mask.rows; j++) {
73 for (
size_t i = 0; i < mask.cols; i++) {
74 if (mask.at<uchar>(j, i) == 255) {
75 indices.push_back(cv::Point(i, j));
79 jsk_recognition_msgs::RectArray rects;
80 rects.header = mask_msg->header;
81 if (indices.size() > 0){
82 cv::Rect mask_rect = cv::boundingRect(indices);
83 jsk_recognition_msgs::Rect rect;
86 rect.width = mask_rect.width;
87 rect.height = mask_rect.height;
88 rects.rects.push_back(rect);
void publish(const boost::shared_ptr< M > &message) const
std::vector< std::string > V_string
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ros::Subscriber sub_mask_
virtual void unsubscribe()
PLUGINLIB_EXPORT_CLASS(jsk_perception::MaskImageToRect, nodelet::Nodelet)
virtual void convert(const sensor_msgs::Image::ConstPtr &mask_msg)