Feature/roi2 (#42)

* Adding settings for ROI specified via a dictionary

* Updating Readme

Author: Vikrant Shah

README.md

@@ -95,8 +95,9 @@ All the parameters can be set via the launch file or via the yaml config_file.
   Flag to flip image horizontally on camera itself, this is not a rotate only a mirror image. This setting does enumeration: "reverseX". It should be specified for all cameras or can be left unspecified for all cameras for default behaviour.
  * ~flip_vertical (bool, default: false)  
   Flag to flip image vertically on camera itself, this is not a rotate only a mirror image. This setting does enumeration: "reverseY". It should be specified for all cameras or can be left unspecified for all cameras for default behaviour.
-  
-  If both horizontal and vertical flags are true, then it is equivalent to rotating 180deg.
+  If both horizontal and vertical flags are true, then it is equivalent to rotating 180deg.	
+  * ~region_of_interest (dict, default = { width: 0,  height: 0, x_offset: 0, y_offset: 0 }
+  Specify the region of interest in the camera image as dict with width, height, x_offset and y_offset. Width and height specify size of the final image (should be smaller than sensor size). X and Y offsets specify the image origin. The offset plus image size should be smaller than the sensor size.
 
 ### node/nodelet Specific Parameters
 * ~tf_prefix (string, default: "")  

include/spinnaker_sdk_camera_driver/capture.h

@@ -132,6 +132,12 @@ namespace acquisition {
         bool PUBLISH_CAM_INFO_;
         bool VERIFY_BINNING_;
         uint64_t SPINNAKER_GET_NEXT_IMAGE_TIMEOUT_;
+        
+        bool region_of_interest_set_;
+        int region_of_interest_width_;
+        int region_of_interest_height_;
+        int region_of_interest_x_offset_;
+        int region_of_interest_y_offset_;
 
         // grid view related variables
         bool GRID_CREATED_;

params/test_params.yaml

@@ -19,8 +19,8 @@ delay: 1.0
 
 #Camera info message details
 distortion_model: plumb_bob
-image_height: 1536
-image_width: 2048
+image_height: 1080
+image_width: 1440
 distortion_coeffs:
 - [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725]
 
@@ -40,3 +40,9 @@ flip_horizontal:
 
 flip_vertical:
 - false
+
+region_of_interest:
+  width: 0
+  height: 0
+  x_offset: 0
+  y_offset: 0

src/capture.cpp

@@ -78,6 +78,7 @@ acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") {
     nframes_ = -1;
     FIXED_NUM_FRAMES_ = false;
     MAX_RATE_SAVE_ = false;
+    region_of_interest_set_ = false;
     skip_num_ = 20; 
     init_delay_ = 1; 
     master_fps_ = 20.0;
@@ -159,7 +160,8 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private
     SAVE_BIN_ = false;
     nframes_ = -1;
     FIXED_NUM_FRAMES_ = false;
-    MAX_RATE_SAVE_ = false;    
+    MAX_RATE_SAVE_ = false;
+    region_of_interest_set_ = false;
     skip_num_ = 20;
     init_delay_ = 1;
     master_fps_ = 20.0;
@@ -187,7 +189,7 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private
 
     GRID_CREATED_ = false;
     VERIFY_BINNING_ = false;
-
+    
 
     //read_settings(config_file);
     read_parameters();
@@ -276,6 +278,14 @@ void acquisition::Capture::load_cameras() {
                 // binning
                 ci_msg->binning_x = binning_;
                 ci_msg->binning_y = binning_;
+                
+                if (region_of_interest_set_ && (region_of_interest_width_!=0 || region_of_interest_height_!=0)){
+                    ci_msg->roi.do_rectify = true;
+                    ci_msg->roi.width = region_of_interest_width_;
+                    ci_msg->roi.height = region_of_interest_height_;
+                    ci_msg->roi.x_offset = region_of_interest_x_offset_;
+                    ci_msg->roi.y_offset = region_of_interest_y_offset_;
+                }
 
                 if (PUBLISH_CAM_INFO_){
                     ci_msg->D = distortion_coeff_vec_[j];
@@ -545,8 +555,26 @@ void acquisition::Capture::read_parameters() {
     }
     else ROS_WARN("  'tf_prefix' Parameter not set, using default behavior tf_prefix=" " ");
 
-    
-
+    if (nh_pvt_.hasParam("region_of_interest")){
+        region_of_interest_set_ = true;
+        if (!nh_pvt_.getParam("region_of_interest/width", region_of_interest_width_)){
+            region_of_interest_set_ = false;
+            }
+        if (!nh_pvt_.getParam("region_of_interest/height", region_of_interest_height_)){
+            region_of_interest_set_ = false;
+            }
+        if (!nh_pvt_.getParam("region_of_interest/x_offset", region_of_interest_x_offset_)){
+            region_of_interest_set_ = false;
+            }
+        if (!nh_pvt_.getParam("region_of_interest/y_offset", region_of_interest_y_offset_)){
+            region_of_interest_set_ = false;
+            }
+        
+        if (region_of_interest_set_){
+            ROS_INFO("  Region of Interest set to width: %d\theight: %d\toffset_x: %d offset_y: %d",
+                     region_of_interest_width_, region_of_interest_height_, region_of_interest_x_offset_, region_of_interest_y_offset_);
+        } else ROS_ERROR("  'region_of_interest' Parameter found but not configured correctly, NOT BEING USED");
+    } else ROS_INFO_STREAM("  'region of interest' not set using full resolution");
 
     bool intrinsics_list_provided = false;
     XmlRpc::XmlRpcValue intrinsics_list;
@@ -677,11 +705,20 @@ void acquisition::Capture::init_cameras(bool soft = false) {
 
                 cams[i].set_color(color_);
                 cams[i].setIntValue("BinningHorizontal", binning_);
-                cams[i].setIntValue("BinningVertical", binning_);
+                cams[i].setIntValue("BinningVertical", binning_);                
                 cams[i].setEnumValue("ExposureMode", "Timed");
                 cams[i].setBoolValue("ReverseX", flip_horizontal_vec_[i]);
                 cams[i].setBoolValue("ReverseY", flip_vertical_vec_[i]);
 
+                if (region_of_interest_set_){
+                    if (region_of_interest_width_ != 0)
+                        cams[i].setIntValue("Width", region_of_interest_width_);
+                    if (region_of_interest_height_ != 0)
+                        cams[i].setIntValue("Height", region_of_interest_height_);
+                    cams[i].setIntValue("OffsetX", region_of_interest_x_offset_);
+                    cams[i].setIntValue("OffsetY", region_of_interest_y_offset_);
+                }
+                
                 if (exposure_time_ > 0) { 
                     cams[i].setEnumValue("ExposureAuto", "Off");
                     cams[i].setFloatValue("ExposureTime", exposure_time_);
@@ -967,7 +1004,9 @@ void acquisition::Capture::run_soft_trig() {
     // Gets called only once, when first image is being triggered
         for (unsigned int i = 0; i < numCameras_; i++) {
             //verify if binning is set successfully
-            ROS_ASSERT_MSG(cams[i].verifyBinning(binning_), " Failed to set Binning= %d, could be either due to Invalid binning value, try changing binning value or due to spinnaker api bug - failing to set lower binning than previously set value - solution: unplug usb camera and re-plug it back and run to node with desired valid binning", binning_);
+            if (!region_of_interest_set_){
+                ROS_ASSERT_MSG(cams[i].verifyBinning(binning_), " Failed to set Binning= %d, could be either due to Invalid binning value, try changing binning value or due to spinnaker api bug - failing to set lower binning than previously set value - solution: unplug usb camera and re-plug it back and run to node with desired valid binning", binning_);
+            }
             // warn if full sensor resolution is not same as calibration resolution
             cams[i].calibrationParamsTest(image_width_,image_height_);
         }