Go to the documentation of this file.00001
00002
00003 #ifndef IMAGE_PROCESSING_NODELET_H_
00004 #define IMAGE_PROCESSING_NODELET_H_
00005
00006 #include <ros/ros.h>
00007 #include <nodelet/nodelet.h>
00008 #include <sensor_msgs/Image.h>
00009 #include <sensor_msgs/CameraInfo.h>
00010
00011 #include <image_transport/image_transport.h>
00012 #include <opencv2/opencv.hpp>
00013 #include <cv_bridge/cv_bridge.h>
00014 #include <std_srvs/Empty.h>
00015 #include <std_msgs/Empty.h>
00016
00017 #include <boost/thread/mutex.hpp>
00018 #include <boost/foreach.hpp>
00019 #include <boost/circular_buffer.hpp>
00020 #include <boost/lambda/lambda.hpp>
00021
00022
00023 #include <dynamic_reconfigure/server.h>
00024 #include <jsk_topic_tools/timered_diagnostic_updater.h>
00025 #include <jsk_topic_tools/diagnostic_utils.h>
00026
00027 namespace resized_image_transport
00028 {
00029 class ImageProcessing : public nodelet::Nodelet
00030 {
00031 public:
00032 protected:
00033 ros::NodeHandle nh;
00034 ros::NodeHandle pnh;
00035
00036
00037 image_transport::CameraSubscriber cs_;
00038 image_transport::CameraPublisher cp_;
00039 image_transport::ImageTransport *it_;
00040 ros::ServiceServer srv_;
00041 ros::Subscriber sub_;
00042
00043 ros::Publisher image_pub_;
00044 ros::Subscriber image_sub_;
00045 image_transport::Subscriber image_nonsync_sub_;
00046 ros::Subscriber camera_info_sub_;
00047 ros::Publisher width_scale_pub_;
00048 ros::Publisher height_scale_pub_;
00049
00050 sensor_msgs::CameraInfoConstPtr info_msg_;
00051
00052 double resize_x_, resize_y_;
00053 int dst_width_, dst_height_;
00054 int max_queue_size_;
00055 bool use_camera_subscriber_;
00056 bool use_snapshot_;
00057 bool publish_once_;
00058 bool use_messages_;
00059 bool use_bytes_;
00060 bool use_camera_info_;
00061 bool verbose_;
00062 ros::Time last_rosinfo_time_, last_subscribe_time_, last_publish_time_;
00063 ros::Duration period_;
00064 boost::mutex mutex_;
00065
00066 boost::circular_buffer<double> in_times;
00067 boost::circular_buffer<double> out_times;
00068 boost::circular_buffer<double> in_bytes;
00069 boost::circular_buffer<double> out_bytes;
00070
00071 jsk_topic_tools::VitalChecker::Ptr image_vital_;
00072 jsk_topic_tools::VitalChecker::Ptr info_vital_;
00073 jsk_topic_tools::TimeredDiagnosticUpdater::Ptr diagnostic_updater_;
00074
00075 virtual void updateDiagnostic(
00076 diagnostic_updater::DiagnosticStatusWrapper &stat);
00077 void onInit();
00078 void initNodeHandle();
00079 void initReconfigure();
00080 void initParams();
00081 void initPublishersAndSubscribers();
00082
00083 public:
00084 ImageProcessing():
00085 in_times(boost::circular_buffer<double>(100)),
00086 out_times(boost::circular_buffer<double>(100)),
00087 in_bytes(boost::circular_buffer<double>(100)),
00088 out_bytes(boost::circular_buffer<double>(100))
00089 { }
00090 ~ImageProcessing() { }
00091
00092 protected:
00093 virtual void process(const sensor_msgs::ImageConstPtr &src_img, const sensor_msgs::CameraInfoConstPtr &src_info,
00094 sensor_msgs::ImagePtr &dst_img, sensor_msgs::CameraInfo &dst_info) = 0;
00095
00096 void snapshot_msg_cb (const std_msgs::EmptyConstPtr msg);
00097
00098 bool snapshot_srv_cb (std_srvs::Empty::Request &req,
00099 std_srvs::Empty::Response &res);
00100
00101 void image_cb(const sensor_msgs::ImageConstPtr &img);
00102 void info_cb(const sensor_msgs::CameraInfoConstPtr &info);
00103 void image_nonsync_cb(const sensor_msgs::ImageConstPtr &img);
00104 void callback(const sensor_msgs::ImageConstPtr &img,
00105 const sensor_msgs::CameraInfoConstPtr &info);
00106 };
00107 }
00108
00109 #endif