Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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
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
00086 private_nh.param("queue_size", queue_size_, 5);
00087
00088
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
00094 image_transport::SubscriberStatusCallback connect_cb = boost::bind(&RectifyNodelet::connectCb, this);
00095
00096 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00097 pub_rect_ = it_->advertise("image_rect", 1, connect_cb, connect_cb);
00098 }
00099
00100
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
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
00125 if (info_msg->D.empty() || info_msg->D[0] == 0.0)
00126 {
00127 pub_rect_.publish(image_msg);
00128 return;
00129 }
00130
00131
00132 model_.fromCameraInfo(info_msg);
00133
00134
00135 const cv::Mat image = cv_bridge::toCvShare(image_msg)->image;
00136 cv::Mat rect;
00137
00138
00139 int interpolation;
00140 {
00141 boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00142 interpolation = config_.interpolation;
00143 }
00144 model_.rectifyImage(image, rect, interpolation);
00145
00146
00147 sensor_msgs::ImagePtr rect_msg = cv_bridge::CvImage(image_msg->header, image_msg->encoding, rect).toImageMsg();
00148 pub_rect_.publish(rect_msg);
00149 }
00150
00151 void RectifyNodelet::configCb(Config &config, uint32_t level)
00152 {
00153 config_ = config;
00154 }
00155
00156 }
00157
00158
00159 #include <pluginlib/class_list_macros.h>
00160 PLUGINLIB_EXPORT_CLASS( image_proc::RectifyNodelet, nodelet::Nodelet)