37 #include <boost/assign.hpp> 39 #include <opencv2/opencv.hpp> 49 DiagnosticNodelet::onInit();
52 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
53 dynamic_reconfigure::Server<Config>::CallbackType
f =
56 srv_->setCallback (f);
57 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
72 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(
queue_size_);
76 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
80 ros::V_string names = boost::assign::list_of(
"~input/color")(
"~input/mono");
91 const sensor_msgs::Image::ConstPtr& mono_imgmsg)
99 if (! ((color_imgmsg->height == mono_imgmsg->height) && (color_imgmsg->width == mono_imgmsg->width))) {
101 10,
"The size of input color and mono image is different: (color: h=%d w=%d), (mono: h=%d w=%d)",
102 color_imgmsg->height, color_imgmsg->width, mono_imgmsg->height, mono_imgmsg->width);
111 cv::Mat overlayed_image = cv::Mat::zeros(color_image.rows, color_image.cols, CV_8UC3);
112 for (
size_t j = 0; j < overlayed_image.rows; j++) {
113 for (
size_t i = 0; i < overlayed_image.cols; i++) {
114 cv::Vec3b color = color_image.at<cv::Vec3b>(j, i);
115 uchar mono = mono_image.at<uchar>(j, i);
122 color_imgmsg->encoding,
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
PLUGINLIB_EXPORT_CLASS(jsk_perception::OverlayImageColorOnMono, nodelet::Nodelet)
void publish(const boost::shared_ptr< M > &message) const
#define NODELET_ERROR_THROTTLE(rate,...)
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
virtual void overlay(const sensor_msgs::Image::ConstPtr &image_msg, const sensor_msgs::Image::ConstPtr &mask_msg)
message_filters::Subscriber< sensor_msgs::Image > sub_mono_
std::vector< std::string > V_string
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
static int numChannels(const std::string &encoding)
virtual void unsubscribe()
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
OverlayImageColorOnMonoConfig Config
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
sensor_msgs::ImagePtr toImageMsg() const
virtual void configCallback(Config &config, uint32_t level)
message_filters::Subscriber< sensor_msgs::Image > sub_color_