warp_image_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 <cv_bridge/cv_bridge.h>
00031 #include <image_transport/image_transport.h>
00032 #include <image_transport/publisher.h>
00033 #include <image_transport/subscriber.h>
00034 #include <opencv2/core/core.hpp>
00035 #include <nodelet/nodelet.h>
00036 #include <ros/ros.h>
00037 #include <sensor_msgs/Image.h>
00038 #include <swri_roscpp/parameters.h>
00039 
00040 namespace swri_image_util
00041 {
00042   class WarpImageNodelet : public nodelet::Nodelet
00043   {
00044     public:
00045       WarpImageNodelet(): use_input_size_(false)
00046       {
00047       }
00048 
00049       ~WarpImageNodelet()
00050       {
00051       }
00052 
00053       void onInit()
00054       {
00055         ros::NodeHandle &node = getNodeHandle();
00056         ros::NodeHandle &priv = getPrivateNodeHandle();
00057 
00058         std::vector<double> transform;
00059         if (priv.hasParam("width") && priv.hasParam("height"))
00060         {
00061           use_input_size_ = false;
00062           swri::getParam(priv, "width", output_size_.width);
00063           swri::getParam(priv, "height", output_size_.height);
00064         }
00065         else
00066         {
00067           use_input_size_ = true;
00068           NODELET_INFO("No ~width and ~height parameters given. Output images will be same size as input.");
00069         }
00070         priv.param("transform", transform, transform);
00071         if (transform.size() != 9)
00072         {
00073           NODELET_FATAL("~transform must be a 9-element list of doubles (3x3 matrix, row major)");
00074           // Return without setting up callbacks
00075           // Don't shut down, because that would bring down all other nodelets as well
00076           return;
00077         }
00078         m_ = cv::Mat(transform, true).reshape(0, 3);
00079         NODELET_INFO_STREAM("Transformation matrix:" << std::endl << m_);
00080 
00081         image_transport::ImageTransport it(node);
00082         image_pub_ = it.advertise("warped_image", 1);
00083         image_sub_ = it.subscribe("image", 1, &WarpImageNodelet::handleImage, this);
00084       }
00085 
00086       void handleImage(sensor_msgs::ImageConstPtr const& image)
00087       {
00088         cv_bridge::CvImageConstPtr cv_image = cv_bridge::toCvShare(image);
00089 
00090         cv_bridge::CvImagePtr cv_warped = boost::make_shared<cv_bridge::CvImage>();
00091         if (use_input_size_)
00092         {
00093           output_size_ = cv_image->image.size();
00094         }
00095         cv::warpPerspective(cv_image->image, cv_warped->image, m_, output_size_, CV_INTER_LANCZOS4);
00096 
00097         cv_warped->encoding = cv_image->encoding;
00098         cv_warped->header = cv_image->header;
00099 
00100         image_pub_.publish(cv_warped->toImageMsg());
00101       }
00102 
00103     private:
00104       image_transport::Subscriber image_sub_;
00105       image_transport::Publisher image_pub_;
00106       cv::Mat m_;
00107       bool use_input_size_;
00108       cv::Size output_size_;
00109   };
00110 }
00111 
00112 // Register nodelet plugin
00113 #include <swri_nodelet/class_list_macros.h>
00114 SWRI_NODELET_EXPORT_CLASS(swri_image_util, WarpImageNodelet)


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