37 #include <boost/assign.hpp> 39 #include <opencv2/opencv.hpp> 42 #if ( CV_MAJOR_VERSION >= 4) 43 #include <opencv2/imgproc/imgproc_c.h> 50 DiagnosticNodelet::onInit();
52 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
53 dynamic_reconfigure::Server<Config>::CallbackType
f =
55 srv_->setCallback (f);
58 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
88 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
91 cv::Mat
label = cv::Mat::zeros(info_msg->height,
98 const sensor_msgs::Image::ConstPtr& image_msg)
101 cv::Mat
label = cv::Mat::zeros(image_msg->height,
109 int num_v = ceil(label.rows / (
float)label_size_);
111 for (
int v = 0;
v < num_v;
v++) {
112 for (
int u = 0; u < num_u; u++) {
113 cv::Rect region(u * label_size_,
v * label_size_,
114 label_size_, label_size_);
115 cv::rectangle(label, region, cv::Scalar(counter), CV_FILLED);
121 label).toImageMsg());
virtual void unsubscribe()
virtual void makeLabel(cv::Mat &label, const std_msgs::Header &header)
void publish(const boost::shared_ptr< M > &message) const
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
std::vector< std::string > V_string
PLUGINLIB_EXPORT_CLASS(jsk_perception::GridLabel, nodelet::Nodelet)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void imageCallback(const sensor_msgs::Image::ConstPtr &image_msg)
const std::string TYPE_32SC1
virtual void configCallback(Config &config, uint32_t level)