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 #include <jsk_topic_tools/diagnostic_nodelet.h>
00027
00028 namespace resized_image_transport
00029 {
00030 class ImageProcessing : public jsk_topic_tools::DiagnosticNodelet
00031 {
00032 public:
00033 protected:
00034
00035 image_transport::CameraSubscriber cs_;
00036 image_transport::CameraPublisher cp_;
00037 image_transport::ImageTransport *it_;
00038 ros::ServiceServer srv_;
00039 ros::Subscriber sub_;
00040
00041 ros::Publisher image_pub_;
00042 ros::Subscriber image_sub_;
00043 image_transport::Subscriber image_nonsync_sub_;
00044 ros::Subscriber camera_info_sub_;
00045 ros::Publisher width_scale_pub_;
00046 ros::Publisher height_scale_pub_;
00047
00048 sensor_msgs::CameraInfoConstPtr info_msg_;
00049
00050 double resize_x_, resize_y_;
00051 int dst_width_, dst_height_;
00052 int max_queue_size_;
00053 bool use_camera_subscriber_;
00054 bool use_snapshot_;
00055 bool publish_once_;
00056 bool use_messages_;
00057 bool use_bytes_;
00058 bool use_camera_info_;
00059 bool verbose_;
00060 ros::Time last_rosinfo_time_, last_subscribe_time_, last_publish_time_;
00061 ros::Duration period_;
00062 boost::mutex mutex_;
00063
00064 boost::circular_buffer<double> in_times;
00065 boost::circular_buffer<double> out_times;
00066 boost::circular_buffer<double> in_bytes;
00067 boost::circular_buffer<double> out_bytes;
00068
00069 jsk_topic_tools::VitalChecker::Ptr image_vital_;
00070 jsk_topic_tools::VitalChecker::Ptr info_vital_;
00071 jsk_topic_tools::TimeredDiagnosticUpdater::Ptr diagnostic_updater_;
00072
00073 virtual void updateDiagnostic(
00074 diagnostic_updater::DiagnosticStatusWrapper &stat);
00075 void onInit();
00076 void initReconfigure();
00077 void initParams();
00078 void initPublishersAndSubscribers();
00079 virtual void subscribe();
00080 virtual void unsubscribe();
00081 public:
00082 ImageProcessing():
00083 in_times(boost::circular_buffer<double>(100)),
00084 out_times(boost::circular_buffer<double>(100)),
00085 in_bytes(boost::circular_buffer<double>(100)),
00086 out_bytes(boost::circular_buffer<double>(100)),
00087 DiagnosticNodelet("ImageProcessing")
00088 { }
00089 ~ImageProcessing() { }
00090
00091 protected:
00092 virtual void process(const sensor_msgs::ImageConstPtr &src_img, const sensor_msgs::CameraInfoConstPtr &src_info,
00093 sensor_msgs::ImagePtr &dst_img, sensor_msgs::CameraInfo &dst_info) = 0;
00094
00095 void snapshot_msg_cb (const std_msgs::EmptyConstPtr msg);
00096
00097 bool snapshot_srv_cb (std_srvs::Empty::Request &req,
00098 std_srvs::Empty::Response &res);
00099
00100 void image_cb(const sensor_msgs::ImageConstPtr &img);
00101 void info_cb(const sensor_msgs::CameraInfoConstPtr &info);
00102 void image_nonsync_cb(const sensor_msgs::ImageConstPtr &img);
00103 void callback(const sensor_msgs::ImageConstPtr &img,
00104 const sensor_msgs::CameraInfoConstPtr &info);
00105 };
00106 }
00107
00108 #endif