34 #include <boost/version.hpp> 35 #if ((BOOST_VERSION / 100) % 1000) >= 53 36 #include <boost/thread/lock_guard.hpp> 44 #include <dynamic_reconfigure/server.h> 45 #include <image_proc/RectifyConfig.h> 61 typedef image_proc::RectifyConfig
Config;
73 void imageCb(
const sensor_msgs::ImageConstPtr& image_msg,
74 const sensor_msgs::CameraInfoConstPtr& info_msg);
76 void configCb(Config &config, uint32_t level);
97 pub_rect_ =
it_->advertise(
"image_rect", 1, connect_cb, connect_cb);
114 const sensor_msgs::CameraInfoConstPtr& info_msg)
117 if (info_msg->K[0] == 0.0) {
125 bool zero_distortion =
true;
126 for (
size_t i = 0; i < info_msg->D.size(); ++i)
128 if (info_msg->D[i] != 0.0)
130 zero_distortion =
false;
151 boost::lock_guard<boost::recursive_mutex> lock(
config_mutex_);
152 interpolation =
config_.interpolation;
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
dynamic_reconfigure::Server< Config > ReconfigureServer
std::string getInfoTopic() const
boost::recursive_mutex config_mutex_
boost::shared_ptr< image_transport::ImageTransport > it_
#define NODELET_ERROR_THROTTLE(rate,...)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
image_transport::Publisher pub_rect_
void imageCb(const sensor_msgs::ImageConstPtr &image_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
boost::shared_ptr< ReconfigureServer > reconfigure_server_
uint32_t getNumSubscribers() const
boost::mutex connect_mutex_
ros::NodeHandle & getPrivateNodeHandle() const
void configCb(Config &config, uint32_t level)
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_
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
image_proc::RectifyConfig Config
ros::NodeHandle & getNodeHandle() const
image_geometry::PinholeCameraModel model_
std::string getTopic() const
sensor_msgs::ImagePtr toImageMsg() const