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 #include <string>
00031 #include <vector>
00032
00033
00034 #include <ros/ros.h>
00035 #include <ros/package.h>
00036 #include <sensor_msgs/Image.h>
00037 #include <cv_bridge/cv_bridge.h>
00038
00039
00040 #include <opencv2/core/core.hpp>
00041 #include <opencv2/highgui/highgui.hpp>
00042
00043
00044 #include <swri_image_util/image_normalization.h>
00045
00046 class NormalizationImageNode
00047 {
00048 private:
00049 ros::NodeHandle nh_;
00050
00051 ros::Subscriber image_sub_;
00052
00053 int32_t num_to_skip_;
00054 int32_t max_num_to_average_;
00055 std::string filename_;
00056
00057 int32_t raw_count_;
00058 int32_t image_count_;
00059
00060 bool image_written_;
00061
00062 std::vector<cv::Mat> image_array_;
00063
00064
00065 void get_parameters()
00066 {
00067 nh_.param(ros::this_node::getName() + "/num_to_skip",
00068 num_to_skip_,
00069 200);
00070
00071 nh_.param(ros::this_node::getName() + "/max_num_to_average",
00072 max_num_to_average_,
00073 100);
00074
00075 std::string temp_filename;
00076 temp_filename = ros::package::getPath("ranger_common") +
00077 "/normalization_image.png";
00078
00079
00080 nh_.param(ros::this_node::getName() + "/filename",
00081 filename_,
00082 temp_filename);
00083
00084 ROS_ERROR("Planning to write normalization image to: %s",
00085 filename_.c_str());
00086 }
00087
00088
00089 void subscribe_to_topics()
00090 {
00091 image_sub_ = nh_.subscribe("image",
00092 2,
00093 &NormalizationImageNode::image_cb,
00094 this);
00095 }
00096
00097 void image_cb(const sensor_msgs::ImageConstPtr& msg)
00098 {
00099 if (image_count_ >= max_num_to_average_)
00100 {
00101 ::sleep(1);
00102 return;
00103 }
00104
00105 if (raw_count_++ % num_to_skip_ == 0)
00106 {
00107 image_count_++;
00108 ROS_ERROR("Got image %d of %d",
00109 image_count_,
00110 max_num_to_average_);
00111
00112 cv_bridge::CvImagePtr im_ptr = cv_bridge::toCvCopy(msg);
00113 cv::Mat image(im_ptr->image);
00114 image_array_.push_back(image);
00115 if (image_count_ >= max_num_to_average_)
00116 {
00117 generate_and_write_image();
00118 }
00119 }
00120 }
00121
00122
00123 void generate_and_write_image()
00124 {
00125 cv::Mat norm_im = swri_image_util::generate_normalization_image(image_array_);
00126 if (!norm_im.empty())
00127 {
00128 try
00129 {
00130 cv::imwrite(filename_, norm_im);
00131 }
00132 catch (const std::exception& e)
00133 {
00134 ROS_ERROR("Failed to save the normalization image: %s",
00135 e.what());
00136 return;
00137 }
00138 ROS_ERROR("Successfully wrote normalization image to: %s",
00139 filename_.c_str());
00140
00141 image_written_ = true;
00142 }
00143 else
00144 {
00145 ROS_ERROR("Failed to generate a normalization image");
00146 }
00147 }
00148
00149 public:
00150 explicit NormalizationImageNode(const ros::NodeHandle& nh):
00151 nh_(nh),
00152 num_to_skip_(20),
00153 max_num_to_average_(100),
00154 raw_count_(0),
00155 image_count_(0),
00156 image_written_(false)
00157 {
00158 filename_ = ros::package::getPath("ranger_common") +
00159 "/normalization_image.png";
00160
00161 get_parameters();
00162
00163 subscribe_to_topics();
00164 }
00165
00166 void shut_down()
00167 {
00168 if (!image_written_ && image_array_.size() > 25)
00169 {
00170 generate_and_write_image();
00171 fprintf(stderr, "\nNode killed before enough frames received to generate "
00172 "normalized image, so a normalized image was generated with "
00173 "available frames (%d vs. %d)\n",
00174 image_count_,
00175 max_num_to_average_);
00176 }
00177 else if (!image_written_)
00178 {
00179 fprintf(stderr, "\nNode killed before enough frames received to generate "
00180 "normalized image: Too few frames in the buffer to generate a "
00181 "normalization image\n");
00182 }
00183 else
00184 {
00185 fprintf(stderr, "\nExiting normally\n");
00186 }
00187 }
00188
00189 bool get_image_written()
00190 {
00191 return image_written_;
00192 }
00193 };
00194
00195 int main(int argc, char **argv)
00196 {
00197 ros::init(argc, argv, "image_normalization_node");
00198
00199 ros::NodeHandle n;
00200
00201 NormalizationImageNode node(n);
00202
00203 ros::spin();
00204
00205 node.shut_down();
00206
00207 return 0;
00208 }