rectify.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #include <boost/version.hpp>
35 #if ((BOOST_VERSION / 100) % 1000) >= 53
36 #include <boost/thread/lock_guard.hpp>
37 #endif
38 
39 #include <ros/ros.h>
40 #include <nodelet/nodelet.h>
43 #include <cv_bridge/cv_bridge.h>
44 #include <dynamic_reconfigure/server.h>
45 #include <image_proc/RectifyConfig.h>
46 
47 namespace image_proc {
48 
50 {
51  // ROS communication
55 
56  boost::mutex connect_mutex_;
58 
59  // Dynamic reconfigure
60  boost::recursive_mutex config_mutex_;
61  typedef image_proc::RectifyConfig Config;
62  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
64  Config config_;
65 
66  // Processing state (note: only safe because we're using single-threaded NodeHandle!)
68 
69  virtual void onInit();
70 
71  void connectCb();
72 
73  void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
74  const sensor_msgs::CameraInfoConstPtr& info_msg);
75 
76  void configCb(Config &config, uint32_t level);
77 };
78 
80 {
82  ros::NodeHandle &private_nh = getPrivateNodeHandle();
83  it_.reset(new image_transport::ImageTransport(nh));
84 
85  // Read parameters
86  private_nh.param("queue_size", queue_size_, 5);
87 
88  // Set up dynamic reconfigure
89  reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
90  ReconfigureServer::CallbackType f = boost::bind(&RectifyNodelet::configCb, this, _1, _2);
91  reconfigure_server_->setCallback(f);
92 
93  // Monitor whether anyone is subscribed to the output
95  // Make sure we don't enter connectCb() between advertising and assigning to pub_rect_
96  boost::lock_guard<boost::mutex> lock(connect_mutex_);
97  pub_rect_ = it_->advertise("image_rect", 1, connect_cb, connect_cb);
98 }
99 
100 // Handles (un)subscribing when clients (un)subscribe
102 {
103  boost::lock_guard<boost::mutex> lock(connect_mutex_);
104  if (pub_rect_.getNumSubscribers() == 0)
106  else if (!sub_camera_)
107  {
109  sub_camera_ = it_->subscribeCamera("image_mono", queue_size_, &RectifyNodelet::imageCb, this, hints);
110  }
111 }
112 
113 void RectifyNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
114  const sensor_msgs::CameraInfoConstPtr& info_msg)
115 {
116  // Verify camera is actually calibrated
117  if (info_msg->K[0] == 0.0) {
118  NODELET_ERROR_THROTTLE(30, "Rectified topic '%s' requested but camera publishing '%s' "
119  "is uncalibrated", pub_rect_.getTopic().c_str(),
120  sub_camera_.getInfoTopic().c_str());
121  return;
122  }
123 
124  // If zero distortion, just pass the message along
125  bool zero_distortion = true;
126  for (size_t i = 0; i < info_msg->D.size(); ++i)
127  {
128  if (info_msg->D[i] != 0.0)
129  {
130  zero_distortion = false;
131  break;
132  }
133  }
134  // This will be true if D is empty/zero sized
135  if (zero_distortion)
136  {
137  pub_rect_.publish(image_msg);
138  return;
139  }
140 
141  // Update the camera model
142  model_.fromCameraInfo(info_msg);
143 
144  // Create cv::Mat views onto both buffers
145  const cv::Mat image = cv_bridge::toCvShare(image_msg)->image;
146  cv::Mat rect;
147 
148  // Rectify and publish
149  int interpolation;
150  {
151  boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
152  interpolation = config_.interpolation;
153  }
154  model_.rectifyImage(image, rect, interpolation);
155 
156  // Allocate new rectified image message
157  sensor_msgs::ImagePtr rect_msg = cv_bridge::CvImage(image_msg->header, image_msg->encoding, rect).toImageMsg();
158  pub_rect_.publish(rect_msg);
159 }
160 
161 void RectifyNodelet::configCb(Config &config, uint32_t level)
162 {
163  config_ = config;
164 }
165 
166 } // namespace image_proc
167 
168 // Register nodelet
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
dynamic_reconfigure::Server< Config > ReconfigureServer
Definition: rectify.cpp:62
std::string getInfoTopic() const
boost::recursive_mutex config_mutex_
Definition: rectify.cpp:60
boost::shared_ptr< image_transport::ImageTransport > it_
Definition: rectify.cpp:52
f
#define NODELET_ERROR_THROTTLE(rate,...)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
image_transport::Publisher pub_rect_
Definition: rectify.cpp:57
void imageCb(const sensor_msgs::ImageConstPtr &image_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
Definition: rectify.cpp:113
virtual void onInit()
Definition: rectify.cpp:79
boost::shared_ptr< ReconfigureServer > reconfigure_server_
Definition: rectify.cpp:63
uint32_t getNumSubscribers() const
boost::mutex connect_mutex_
Definition: rectify.cpp:56
ros::NodeHandle & getPrivateNodeHandle() const
void configCb(Config &config, uint32_t level)
Definition: rectify.cpp:161
void rectifyImage(const cv::Mat &raw, cv::Mat &rectified, int interpolation=cv::INTER_LINEAR) const
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
void publish(const sensor_msgs::Image &message) const
image_transport::CameraSubscriber sub_camera_
Definition: rectify.cpp:53
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
image_proc::RectifyConfig Config
Definition: rectify.cpp:61
ros::NodeHandle & getNodeHandle() const
image_geometry::PinholeCameraModel model_
Definition: rectify.cpp:67
std::string getTopic() const
sensor_msgs::ImagePtr toImageMsg() const


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Thu Nov 7 2019 03:44:58