Feature/cam info msg fix (#39)

* minor changes to adding camerainfo header in export_to_ros method

* will handle if binning is set properly and wwarn if camera is not calibrated at full resolution

Author: mithundiddi

include/spinnaker_sdk_camera_driver/camera.h

@@ -64,6 +64,8 @@ namespace acquisition {
         bool is_master() { return MASTER_; }
         void set_color(bool flag) { COLOR_ = flag; }
         void setGetNextImageTimeout(uint64_t get_next_image_timeout) { GET_NEXT_IMAGE_TIMEOUT_ = get_next_image_timeout; }
+        bool verifyBinning(int binningDesired);
+        void calibrationParamsTest(int calibrationWidth, int calibrationHeight);
 
     private:
 

include/spinnaker_sdk_camera_driver/capture.h

@@ -110,6 +110,8 @@ namespace acquisition {
 
         int MASTER_CAM_;
         int CAM_; // active cam during live
+        int image_width_;
+        int image_height_;
 
         bool FIXED_NUM_FRAMES_;
         bool TIME_BENCHMARK_;
@@ -126,6 +128,7 @@ namespace acquisition {
         bool EXPORT_TO_ROS_;
         bool MAX_RATE_SAVE_;
         bool PUBLISH_CAM_INFO_;
+        bool VERIFY_BINNING_;
         uint64_t SPINNAKER_GET_NEXT_IMAGE_TIMEOUT_;
 
         // grid view related variables

params/stereo_camera_example.yaml

@@ -21,8 +21,8 @@ delay: 1.0
 
 #Camera info message details
 distortion_model: plumb_bob
-image_height: 1536
-image_width: 2048
+image_height: 1024 #1536
+image_width: 1280 #2048
 distortion_coeffs:
 - [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725]
 - [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725]

params/test_params.yaml

@@ -1,8 +1,8 @@
 cam_ids:
-- 17197559
+- 18080146
 cam_aliases:
 - cam0
-master_cam: 17197559
+master_cam: 18080146
 skip: 20
 delay: 1.0
 

src/camera.cpp

@@ -316,3 +316,16 @@ void acquisition::Camera::exposureTest() {
     ROS_DEBUG_STREAM("Exposure Time: "<<expTime<<endl);
 
 }
+bool acquisition::Camera::verifyBinning(int binningDesired) {
+    int actualBinningX =  (pCam_ ->SensorWidth())/(pCam_ ->Width());
+    int actualBinningY =  (pCam_ ->SensorHeight())/(pCam_ ->Height());
+    if (binningDesired == actualBinningX) return true;
+    else return false;
+}
+
+void acquisition::Camera::calibrationParamsTest(int calibrationWidth, int calibrationHeight) {
+    if ( (pCam_ ->SensorWidth()) != calibrationWidth )
+        ROS_WARN_STREAM(" Looks like your calibration is not done at full Sensor Resolution for cam_id = "<<get_id()<<" , Sensor_Width = "<<(pCam_ ->SensorWidth()) <<" given cameraInfo params:width = "<<calibrationWidth);
+    if ( (pCam_ ->SensorHeight()) != calibrationHeight )
+        ROS_WARN_STREAM(" Looks like your calibration is not done at full Sensor Resolution for cam_id = "<<get_id()<<" , Sensor_Height= "<<(pCam_ ->SensorHeight()) <<" given cameraInfo params:height = "<<calibrationHeight);
+}

src/capture.cpp

@@ -104,6 +104,7 @@ acquisition::Capture::Capture(): it_(nh_), nh_pvt_ ("~") {
     CAM_DIRS_CREATED_ = false;
 
     GRID_CREATED_ = false;
+    VERIFY_BINNING_ = false;
 
 
     //read_settings(config_file);
@@ -185,6 +186,7 @@ acquisition::Capture::Capture(ros::NodeHandle nodehandl, ros::NodeHandle private
     CAM_DIRS_CREATED_ = false;
 
     GRID_CREATED_ = false;
+    VERIFY_BINNING_ = false;
 
 
     //read_settings(config_file);
@@ -227,7 +229,6 @@ void acquisition::Capture::load_cameras() {
     bool master_set = false;
     int cam_counter = 0;
 
-    
     for (int j=0; j<cam_ids_.size(); j++) {
         bool current_cam_found=false;
         for (int i=0; i<numCameras_; i++) {
@@ -260,16 +261,16 @@ void acquisition::Capture::load_cameras() {
                 img_msgs.push_back(sensor_msgs::ImagePtr());
 
                 sensor_msgs::CameraInfoPtr ci_msg(new sensor_msgs::CameraInfo());
-                int image_width = 0;
-                int image_height = 0;
+                //int image_width = 0;
+                //int image_height = 0;
+                nh_pvt_.getParam("image_height", image_height_);
+                nh_pvt_.getParam("image_width", image_width_);
+                // full resolution image_size
+                ci_msg->height = image_height_;
+                ci_msg->width = image_width_;
+                                
                 std::string distortion_model = ""; 
-                nh_pvt_.getParam("image_height", image_height);
-                nh_pvt_.getParam("image_width", image_width);
                 nh_pvt_.getParam("distortion_model", distortion_model);
-                ci_msg->header.frame_id = "cam_"+to_string(j)+"_optical_frame";
-                // full resolution image_size
-                ci_msg->height = image_height;
-                ci_msg->width = image_width;
                 // distortion
                 ci_msg->distortion_model = distortion_model;
                 // binning
@@ -316,13 +317,13 @@ void acquisition::Capture::load_cameras() {
                 cam_info_msgs.push_back(ci_msg);
 
                 cam_counter++;
-            
+                
             }
         }
         if (!current_cam_found) ROS_WARN_STREAM("   Camera "<<cam_ids_[j]<<" not detected!!!");
     }
     ROS_ASSERT_MSG(cams.size(),"None of the connected cameras are in the config list!");
-    
+
     ROS_ASSERT_MSG(master_set,"The camera supposed to be the master isn't connected!");
     // Setting numCameras_ variable to reflect number of camera objects used.
     // numCameras_ variable is used in other methods where it means size of cams list.
@@ -813,23 +814,15 @@ void acquisition::Capture::export_to_ROS() {
 
     for (unsigned int i = 0; i < numCameras_; i++) {
         img_msg_header.frame_id = frame_id_prefix + "cam_"+to_string(i)+"_optical_frame";
+        cam_info_msgs[i]->header = img_msg_header;
 
         if(color_)
             img_msgs[i]=cv_bridge::CvImage(img_msg_header, "bgr8", frames_[i]).toImageMsg();
         else
             img_msgs[i]=cv_bridge::CvImage(img_msg_header, "mono8", frames_[i]).toImageMsg();
 
-        if (PUBLISH_CAM_INFO_){
-            cam_info_msgs[i]->header.stamp = mesg.header.stamp;
-            cam_info_msgs[i]->header.frame_id = frame_id_prefix + "cam_"+to_string(i)+"_optical_frame";
-        }
         camera_image_pubs[i].publish(img_msgs[i],cam_info_msgs[i]);
-/*
-        if (PUBLISH_CAM_INFO_){
-        cam_info_msgs[i].header.stamp = mesg.header.stamp;
-        camera_info_pubs[i].publish(cam_info_msgs[i]);
-        }
-    */
+
     }
     export_to_ROS_time_ = ros::Time::now().toSec()-t;;
 }
@@ -940,6 +933,18 @@ void acquisition::Capture::run_soft_trig() {
             save_mat_frames(0);
     }
 
+    if(!VERIFY_BINNING_){
+    // 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_);
+            // warn if full sensor resolution is not same as calibration resolution
+            cams[i].calibrationParamsTest(image_width_,image_height_);
+        }
+    VERIFY_BINNING_ = true;
+    }
+
+
     ros::Rate ros_rate(soft_framerate_);
     try{
         while( ros::ok() ) {
@@ -1018,7 +1023,7 @@ void acquisition::Capture::run_soft_trig() {
                     break;
                 }
             }
-            
+
             if (EXPORT_TO_ROS_) export_to_ROS();
             //cams[MASTER_CAM_].targetGreyValueTest();
             // ros publishing messages