37 #include <boost/assign.hpp> 41 #if ( CV_MAJOR_VERSION >= 4) 42 #include <opencv2/imgproc/imgproc_c.h> 49 DiagnosticNodelet::onInit();
50 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
59 ros::V_string names = boost::assign::list_of(
"~input")(
"~input/camera_info");
70 const geometry_msgs::PolygonStamped::ConstPtr& rect_msg)
74 cv::Mat mask_image = cv::Mat::zeros(
camera_info_->height,
77 geometry_msgs::Point32 P0 = rect_msg->polygon.points[0];
78 geometry_msgs::Point32 P1 = rect_msg->polygon.points[2];
79 double min_x = std::max(std::min(P0.x, P1.x), 0.0f);
80 double max_x = std::max(P0.x, P1.x);
81 double min_y = std::max(std::min(P0.y, P1.y), 0.0f);
82 double max_y = std::max(P0.y, P1.y);
85 cv::Rect region(min_x, min_y, width, height);
86 cv::rectangle(mask_image, region, cv::Scalar(255), CV_FILLED);
96 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber sub_info_
virtual void convert(const geometry_msgs::PolygonStamped::ConstPtr &rect_msg)
std::vector< std::string > V_string
PLUGINLIB_EXPORT_CLASS(jsk_perception::RectToMaskImage, nodelet::Nodelet)
virtual void unsubscribe()
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
sensor_msgs::CameraInfo::ConstPtr camera_info_
sensor_msgs::ImagePtr toImageMsg() const