blend_images_nodelet.cpp
Go to the documentation of this file.
00001 // *****************************************************************************
00002 //
00003 // Copyright (c) 2017, Southwest Research Institute® (SwRI®)
00004 // All rights reserved.
00005 //
00006 // Redistribution and use in source and binary forms, with or without
00007 // modification, are permitted provided that the following conditions are met:
00008 //     * Redistributions of source code must retain the above copyright
00009 //       notice, this list of conditions and the following disclaimer.
00010 //     * Redistributions in binary form must reproduce the above copyright
00011 //       notice, this list of conditions and the following disclaimer in the
00012 //       documentation and/or other materials provided with the distribution.
00013 //     * Neither the name of Southwest Research Institute® (SwRI®) nor the
00014 //       names of its contributors may be used to endorse or promote products
00015 //       derived from this software without specific prior written permission.
00016 //
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //
00028 // *****************************************************************************
00029 
00030 #include <opencv2/core/core.hpp>
00031 
00032 #include <ros/ros.h>
00033 #include <cv_bridge/cv_bridge.h>
00034 #include <nodelet/nodelet.h>
00035 #include <image_transport/image_transport.h>
00036 #include <message_filters/subscriber.h>
00037 #include <message_filters/time_synchronizer.h>
00038 #include <message_filters/sync_policies/approximate_time.h>
00039 #include <sensor_msgs/Image.h>
00040 #include <swri_opencv_util/blend.h>
00041 
00042 namespace swri_image_util
00043 {
00044   const double DEFAULT_ALPHA_LEVEL = 0.5;
00045   const cv::Scalar NO_MASK = cv::Scalar(-1, -1, -1);
00046 
00047   // ROS nodelet for blending the images together
00048   class BlendImagesNodelet : public nodelet::Nodelet
00049   {
00050   public:
00051       BlendImagesNodelet();
00052       ~BlendImagesNodelet();
00053   private:
00054       // Use the message_filters to listen for the base and top image
00055       // using an approximate time synchronization policy. These typedefs
00056       // make creating the filter cleaner looking
00057       typedef message_filters::sync_policies::ApproximateTime<
00058         sensor_msgs::Image, sensor_msgs::Image> ApproximateTimePolicy;
00059       typedef message_filters::Synchronizer<ApproximateTimePolicy>
00060         ApproximateTimeSync;
00061       // Called when the nodelet starts up. This does all of the initialization
00062       virtual void onInit();
00063       // Callback for the two images we are blending together
00064       void imageCallback(
00065           const sensor_msgs::ImageConstPtr &base_image,
00066           const sensor_msgs::ImageConstPtr &top_image);
00067       // Alpha blending level of the two images
00068       double alpha_;
00069       // Color to mask, if necessary
00070       cv::Scalar mask_color_;
00071       // Publishes the blended image
00072       image_transport::Publisher image_pub_;
00073       // The subscribers for the base and top image
00074       message_filters::Subscriber<sensor_msgs::Image> base_image_sub_;
00075       message_filters::Subscriber<sensor_msgs::Image> top_image_sub_;
00076       // Synchronization object for the two images we need for the blending
00077       // process
00078       boost::shared_ptr<ApproximateTimeSync> image_sync_;
00079   };
00080 
00081   BlendImagesNodelet::BlendImagesNodelet() :
00082     alpha_(DEFAULT_ALPHA_LEVEL),
00083     mask_color_(NO_MASK)
00084   {
00085   }
00086 
00087   BlendImagesNodelet::~BlendImagesNodelet()
00088   {
00089   }
00090 
00091   void BlendImagesNodelet::onInit()
00092   {
00093     // Node handles for interacting with the wider ROS system
00094     ros::NodeHandle& node = getNodeHandle();
00095     ros::NodeHandle& priv = getPrivateNodeHandle();
00096 
00097     // User setting for the alpha value. The constructor should have
00098     // already set this to the default value
00099     priv.param("alpha", alpha_, alpha_);
00100 
00101     // Values should be in the range [0,255]
00102     double mask_r;
00103     double mask_g;
00104     double mask_b;
00105     priv.param("mask_r", mask_r, -1.0);
00106     priv.param("mask_g", mask_g, -1.0);
00107     priv.param("mask_b", mask_b, -1.0);
00108 
00109     // Only create the mask if all components are valid
00110     if ((mask_r >= 0) && (mask_g >= 0) && (mask_b >= 0) &&
00111         (mask_r <= 255) && (mask_g <= 255) && (mask_b <= 255))
00112     {
00113       mask_color_ = cv::Scalar(mask_r, mask_g, mask_b);
00114     }
00115     else
00116     {
00117       ROS_ERROR("Mask color components must be in range [0,255]");
00118       ROS_ERROR("  Components were (%f, %f, %f)", mask_r, mask_g, mask_b);
00119     }
00120 
00121     // Set up our publisher of the blended data and listen to the two input
00122     // images
00123     image_transport::ImageTransport it(node);
00124     image_pub_ = it.advertise("blended_image", 1);
00125     base_image_sub_.subscribe(node, "base_image", 1);
00126     top_image_sub_.subscribe(node, "top_image", 1);
00127     image_sync_.reset(new ApproximateTimeSync(
00128                         ApproximateTimePolicy(10),
00129                         base_image_sub_,
00130                         top_image_sub_));
00131     // Start listening for the images
00132     image_sync_->registerCallback(boost::bind(
00133                                     &BlendImagesNodelet::imageCallback,
00134                                     this,
00135                                     _1,
00136                                     _2));
00137   }
00138 
00139   void BlendImagesNodelet::imageCallback(
00140       const sensor_msgs::ImageConstPtr& base_image,
00141       const sensor_msgs::ImageConstPtr& top_image)
00142   {
00143     // Convert the ROS image types to OpenCV types. Use toCvShare() here because
00144     // the base and top image will not be modified, and so we do not need our
00145     // own copy of the image
00146     cv_bridge::CvImageConstPtr cv_base_image = cv_bridge::toCvShare(base_image);
00147     // Use the base image encoding during the conversion so different types of
00148     // images can be blended together
00149     cv_bridge::CvImageConstPtr cv_top_image = cv_bridge::toCvShare(
00150           top_image,
00151           base_image->encoding);
00152 
00153     // Initialize the output to the same size and type as the base image
00154     cv::Mat blended = cv::Mat::zeros(
00155           cv_base_image->image.rows,
00156           cv_base_image->image.cols,
00157           cv_base_image->image.type());
00158 
00159     // Blend the images together
00160     if (mask_color_ != NO_MASK)
00161     {
00162       cv::Mat mask;
00163       cv::inRange(cv_top_image->image, mask_color_, mask_color_, mask);
00164 
00165       blended = swri_opencv_util::overlayColor(
00166             cv_base_image->image,
00167             mask,
00168             mask_color_,
00169             alpha_);
00170     }
00171     else
00172     {
00173       blended = swri_opencv_util::blend(
00174             cv_top_image->image,
00175             cv_base_image->image,
00176             alpha_);
00177     }
00178 
00179     // Convert the blended image to a ROS type and publish the result
00180     cv_bridge::CvImagePtr cv_blended = boost::make_shared<cv_bridge::CvImage>();
00181     cv_blended->image = blended;
00182     cv_blended->encoding = cv_base_image->encoding;
00183     cv_blended->header = cv_base_image->header;
00184 
00185     image_pub_.publish(cv_blended->toImageMsg());
00186   }
00187 }
00188 
00189 // Register nodelet plugin
00190 #include <swri_nodelet/class_list_macros.h>
00191 SWRI_NODELET_EXPORT_CLASS(swri_image_util, BlendImagesNodelet)


swri_image_util
Author(s): Kris Kozak
autogenerated on Thu Jun 6 2019 20:34:52