00001 #include <ros/ros.h>
00002 #include <nodelet/nodelet.h>
00003 #include <image_transport/image_transport.h>
00004 #include <image_geometry/pinhole_camera_model.h>
00005 #include <cv_bridge/CvBridge.h>
00006 #include <dynamic_reconfigure/server.h>
00007 #include <image_proc/RectifyConfig.h>
00008
00009 namespace image_proc {
00010
00011 class RectifyNodelet : public nodelet::Nodelet
00012 {
00013
00014 boost::shared_ptr<image_transport::ImageTransport> it_;
00015 image_transport::CameraSubscriber sub_camera_;
00016 int queue_size_;
00017
00018 boost::mutex connect_mutex_;
00019 image_transport::Publisher pub_rect_;
00020
00021
00022 boost::recursive_mutex config_mutex_;
00023 typedef image_proc::RectifyConfig Config;
00024 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00025 boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00026 Config config_;
00027
00028
00029 image_geometry::PinholeCameraModel model_;
00030
00031 virtual void onInit();
00032
00033 void connectCb();
00034
00035 void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
00036 const sensor_msgs::CameraInfoConstPtr& info_msg);
00037
00038 void configCb(Config &config, uint32_t level);
00039 };
00040
00041 void RectifyNodelet::onInit()
00042 {
00043 ros::NodeHandle &nh = getNodeHandle();
00044 ros::NodeHandle &private_nh = getPrivateNodeHandle();
00045 it_.reset(new image_transport::ImageTransport(nh));
00046
00047
00048 private_nh.param("queue_size", queue_size_, 5);
00049
00050
00051 reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
00052 ReconfigureServer::CallbackType f = boost::bind(&RectifyNodelet::configCb, this, _1, _2);
00053 reconfigure_server_->setCallback(f);
00054
00055
00056 image_transport::SubscriberStatusCallback connect_cb = boost::bind(&RectifyNodelet::connectCb, this);
00057
00058 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00059 pub_rect_ = it_->advertise("image_rect", 1, connect_cb, connect_cb);
00060 }
00061
00062
00063 void RectifyNodelet::connectCb()
00064 {
00065 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00066 if (pub_rect_.getNumSubscribers() == 0)
00067 sub_camera_.shutdown();
00068 else if (!sub_camera_)
00069 sub_camera_ = it_->subscribeCamera("image_mono", queue_size_, &RectifyNodelet::imageCb, this);
00070 }
00071
00072 void RectifyNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
00073 const sensor_msgs::CameraInfoConstPtr& info_msg)
00074 {
00075
00076 if (info_msg->K[0] == 0.0) {
00077 NODELET_ERROR_THROTTLE(30, "Rectified topic '%s' requested but camera publishing '%s' "
00078 "is uncalibrated", pub_rect_.getTopic().c_str(),
00079 sub_camera_.getInfoTopic().c_str());
00080 return;
00081 }
00082
00083
00084 if (info_msg->D.empty() || info_msg->D[0] == 0.0)
00085 {
00086 pub_rect_.publish(image_msg);
00087 return;
00088 }
00089
00090
00091 model_.fromCameraInfo(info_msg);
00092
00093
00094 sensor_msgs::ImagePtr rect_msg = boost::make_shared<sensor_msgs::Image>();
00095 rect_msg->header = image_msg->header;
00096 rect_msg->height = image_msg->height;
00097 rect_msg->width = image_msg->width;
00098 rect_msg->encoding = image_msg->encoding;
00099 rect_msg->step = image_msg->step;
00100 rect_msg->data.resize(rect_msg->height * rect_msg->step);
00101
00102
00103 sensor_msgs::CvBridge image_bridge, rect_bridge;
00104 const cv::Mat image = image_bridge.imgMsgToCv(image_msg);
00105 cv::Mat rect = rect_bridge.imgMsgToCv(rect_msg);
00106
00107
00108 int interpolation;
00109 {
00110 boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00111 interpolation = config_.interpolation;
00112 }
00113 model_.rectifyImage(image, rect, interpolation);
00114 pub_rect_.publish(rect_msg);
00115 }
00116
00117 void RectifyNodelet::configCb(Config &config, uint32_t level)
00118 {
00119 config_ = config;
00120 }
00121
00122 }
00123
00124
00125 #include <pluginlib/class_list_macros.h>
00126 PLUGINLIB_DECLARE_CLASS(image_proc, rectify, image_proc::RectifyNodelet, nodelet::Nodelet)