rectify.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <boost/version.hpp>
00035 #if ((BOOST_VERSION / 100) % 1000) >= 53
00036 #include <boost/thread/lock_guard.hpp>
00037 #endif
00038 
00039 #include <ros/ros.h>
00040 #include <nodelet/nodelet.h>
00041 #include <image_transport/image_transport.h>
00042 #include <image_geometry/pinhole_camera_model.h>
00043 #include <cv_bridge/cv_bridge.h>
00044 #include <dynamic_reconfigure/server.h>
00045 #include <image_proc/RectifyConfig.h>
00046 
00047 namespace image_proc {
00048 
00049 class RectifyNodelet : public nodelet::Nodelet
00050 {
00051   // ROS communication
00052   boost::shared_ptr<image_transport::ImageTransport> it_;
00053   image_transport::CameraSubscriber sub_camera_;
00054   int queue_size_;
00055   
00056   boost::mutex connect_mutex_;
00057   image_transport::Publisher pub_rect_;
00058 
00059   // Dynamic reconfigure
00060   boost::recursive_mutex config_mutex_;
00061   typedef image_proc::RectifyConfig Config;
00062   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00063   boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00064   Config config_;
00065 
00066   // Processing state (note: only safe because we're using single-threaded NodeHandle!)
00067   image_geometry::PinholeCameraModel model_;
00068 
00069   virtual void onInit();
00070 
00071   void connectCb();
00072 
00073   void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
00074                const sensor_msgs::CameraInfoConstPtr& info_msg);
00075 
00076   void configCb(Config &config, uint32_t level);
00077 };
00078 
00079 void RectifyNodelet::onInit()
00080 {
00081   ros::NodeHandle &nh         = getNodeHandle();
00082   ros::NodeHandle &private_nh = getPrivateNodeHandle();
00083   it_.reset(new image_transport::ImageTransport(nh));
00084 
00085   // Read parameters
00086   private_nh.param("queue_size", queue_size_, 5);
00087 
00088   // Set up dynamic reconfigure
00089   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
00090   ReconfigureServer::CallbackType f = boost::bind(&RectifyNodelet::configCb, this, _1, _2);
00091   reconfigure_server_->setCallback(f);
00092 
00093   // Monitor whether anyone is subscribed to the output
00094   image_transport::SubscriberStatusCallback connect_cb = boost::bind(&RectifyNodelet::connectCb, this);
00095   // Make sure we don't enter connectCb() between advertising and assigning to pub_rect_
00096   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00097   pub_rect_  = it_->advertise("image_rect",  1, connect_cb, connect_cb);
00098 }
00099 
00100 // Handles (un)subscribing when clients (un)subscribe
00101 void RectifyNodelet::connectCb()
00102 {
00103   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00104   if (pub_rect_.getNumSubscribers() == 0)
00105     sub_camera_.shutdown();
00106   else if (!sub_camera_)
00107   {
00108     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00109     sub_camera_ = it_->subscribeCamera("image_mono", queue_size_, &RectifyNodelet::imageCb, this, hints);
00110   }
00111 }
00112 
00113 void RectifyNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
00114                              const sensor_msgs::CameraInfoConstPtr& info_msg)
00115 {
00116   // Verify camera is actually calibrated
00117   if (info_msg->K[0] == 0.0) {
00118     NODELET_ERROR_THROTTLE(30, "Rectified topic '%s' requested but camera publishing '%s' "
00119                            "is uncalibrated", pub_rect_.getTopic().c_str(),
00120                            sub_camera_.getInfoTopic().c_str());
00121     return;
00122   }
00123 
00124   // If zero distortion, just pass the message along
00125   bool zero_distortion = true;
00126   for (size_t i = 0; i < info_msg->D.size(); ++i)
00127   {
00128     if (info_msg->D[i] != 0.0)
00129     {
00130       zero_distortion = false;
00131       break;
00132     }
00133   }
00134   // This will be true if D is empty/zero sized
00135   if (zero_distortion)
00136   {
00137     pub_rect_.publish(image_msg);
00138     return;
00139   }
00140 
00141   // Update the camera model
00142   model_.fromCameraInfo(info_msg);
00143   
00144   // Create cv::Mat views onto both buffers
00145   const cv::Mat image = cv_bridge::toCvShare(image_msg)->image;
00146   cv::Mat rect;
00147 
00148   // Rectify and publish
00149   int interpolation;
00150   {
00151     boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00152     interpolation = config_.interpolation;
00153   }
00154   model_.rectifyImage(image, rect, interpolation);
00155 
00156   // Allocate new rectified image message
00157   sensor_msgs::ImagePtr rect_msg = cv_bridge::CvImage(image_msg->header, image_msg->encoding, rect).toImageMsg();
00158   pub_rect_.publish(rect_msg);
00159 }
00160 
00161 void RectifyNodelet::configCb(Config &config, uint32_t level)
00162 {
00163   config_ = config;
00164 }
00165 
00166 } // namespace image_proc
00167 
00168 // Register nodelet
00169 #include <pluginlib/class_list_macros.h>
00170 PLUGINLIB_EXPORT_CLASS( image_proc::RectifyNodelet, nodelet::Nodelet)


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Tue Sep 19 2017 02:56:13