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
00031 #include <ros/ros.h>
00032 #include <unistd.h>
00033 #include <opencv/cv.h>
00034 #include <opencv/highgui.h>
00035 #include <sensor_msgs/Image.h>
00036 #include <sensor_msgs/RegionOfInterest.h>
00037 #include <cv_bridge/CvBridge.h>
00038 #include "ias_projected_light/cp.h"
00039 #include <math.h>
00040
00041
00042
00043 using namespace std;
00044
00045 class AdjExposure
00046 {
00047 private:
00048 ros::NodeHandle n;
00049 ros::Subscriber sub;
00050 ros::Subscriber sub_roi;
00051 ros::ServiceClient cl;
00052
00053 string prefix;
00054 string input_topic;
00055 string roi_input_topic;
00056
00057 IplImage* image;
00058 IplImage* non_image;
00059
00060 CvRect roi;
00061
00062 sensor_msgs::CvBridge bridge;
00063
00064 int exposure_min, exposure_max, exposure;
00065 int mode;
00066
00067 bool roi_set;
00068
00069
00070 const static int avg_brightness_auto = 120;
00071
00072
00073 const static int pattern_number = 4;
00074
00075
00076
00077 void setExposure()
00078 {
00079 stringstream ss;
00080 ss << "rosrun dynamic_reconfigure dynparam set " << prefix << " exposure " << exposure;
00081 int s = system(ss.str().c_str());
00082 if (s != 0)
00083 {
00084 exit(s);
00085 }
00086
00087 }
00088
00089 void changeROI(const sensor_msgs::RegionOfInterestConstPtr& r)
00090 {
00091 CvRect roi_new;
00092 roi_new.x = r->x_offset;
00093 roi_new.y = r->y_offset;
00094 roi_new.width = r->width;
00095 roi_new.height = r->height;
00096 roi = roi_new;
00097
00098 roi_set = true;
00099 }
00100
00101 void adjust(const sensor_msgs::ImageConstPtr& ms)
00102 {
00103 if (!roi_set)
00104 return;
00105
00106 image = bridge.imgMsgToCv(ms);
00107
00108 switch (mode)
00109 {
00110 case 0:
00111 {
00112 ias_projected_light::cp srv;
00113 srv.request.pattern = 0;
00114 srv.request.block_size = 1;
00115 while (ros::ok() && !cl.call(srv))
00116 {
00117 ROS_INFO("Failed to show black pattern!");
00118 cvWaitKey(50);
00119 }
00120 ROS_INFO("Switched to black image!");
00121 sleep(1);
00122 }
00123 break;
00124
00125 case 1:
00126 {
00127 non_image = cvCreateImage(cvSize(image->width, image->height), image->depth, 1);
00128 cvCopy(image, non_image);
00129 ias_projected_light::cp srv;
00130 srv.request.pattern = pattern_number;
00131 srv.request.block_size = 15;
00132 while (ros::ok() && !cl.call(srv))
00133 {
00134 ROS_INFO("Failed to show pattern!");
00135
00136 cvWaitKey(50);
00137 }
00138 ROS_INFO("Switched to pattern!");
00139 sleep(1);
00140 }
00141 break;
00142
00143 default:
00144 {
00145 int w = roi.width;
00146 int h = roi.height;
00147
00148
00149
00150 int avg_image = 0;
00151 int avg_roi = 0;
00152
00153 for (int i = 0; i < image->width; i++)
00154 {
00155 for (int j = 0; j < image->height; j++)
00156 {
00157 if (i >= roi.x && i <= roi.x + roi.width && j >= roi.y && j <= roi.y + roi.height)
00158 {
00159 avg_roi += cvGet2D(image, j, i).val[0];
00160 }
00161 avg_image += cvGet2D(image, j, i).val[0];
00162 }
00163 }
00164 avg_roi /= (w * h);
00165 avg_image /= (image->width * image->height);
00166
00167 int avg_brightness_wanted = avg_image + avg_brightness_auto - avg_roi;
00168
00169 exposure = max(1, avg_brightness_wanted * exposure / avg_image);
00170 setExposure();
00171 ROS_INFO("Wanted Avg: %d Real Avg: %d, Avg ROI (should be %d):, %d", avg_brightness_wanted, avg_image,
00172 avg_brightness_auto, avg_roi);
00173
00174 }
00175 }
00176 if (mode < 3)
00177 mode++;
00178 }
00179
00180 public:
00181 AdjExposure(ros::NodeHandle nh) :
00182 n(nh), roi_set(false)
00183 {
00184 prefix = nh.resolveName("prefix");
00185 if (strcmp(prefix.c_str(), "/prefix") == 0)
00186 {
00187 prefix = "/stereo/stereodcam2701/";
00188 }
00189
00190 input_topic = nh.resolveName("input");
00191 if (strcmp(input_topic.c_str(), "/input") == 0)
00192 {
00193 input_topic = "/stereo/left/image_mono";
00194 }
00195 roi_input_topic = nh.resolveName("roi_input");
00196 if (strcmp(roi_input_topic.c_str(), "/roi_input") == 0)
00197 {
00198 roi_input_topic = "/stereo/left/roi";
00199 }
00200 mode = 0;
00201
00202
00203 if (!nh.getParam(prefix + "exposure_min", exposure_min))
00204 {
00205 ROS_ERROR("Parameter %sexposure_min not found!", prefix.c_str());
00206 exit(-1);
00207 }
00208 if (!nh.getParam(prefix + "exposure_max", exposure_max))
00209 {
00210 ROS_ERROR("Parameter %sexposure_max not found!", prefix.c_str());
00211 exit(-1);
00212 }
00213 if (!nh.getParam(prefix + "exposure", exposure))
00214 {
00215 ROS_ERROR("Parameter %sexposure not found!", prefix.c_str());
00216 exit(-1);
00217 }
00218
00219 ROS_INFO("Exposure parameters found!");
00220
00221 exposure = (exposure_max - exposure_min) / 2;
00222 setExposure();
00223
00224 sleep(2);
00225
00226 sub = nh.subscribe(input_topic, 1, &AdjExposure::adjust, this);
00227 sub_roi = nh.subscribe(roi_input_topic, 1, &AdjExposure::changeROI, this);
00228 cl = nh.serviceClient<ias_projected_light::cp> ("change_pattern");
00229
00230 }
00231 };
00232
00233 int main(int argc, char** argv)
00234 {
00235 ros::init(argc, argv, "adjust_exposure");
00236 ros::NodeHandle nh;
00237
00238 AdjExposure a(nh);
00239
00240 ros::spin();
00241 }