fixes issue when more cameras are plugged into host than count of spe…

…cified cam_ids, node will not die, and now node publishes empty intrinsics in camerainfo msg when not provided, a bug from pr #33 where default was to not publish info msg (#36)

fixes issue when more cameras are plugged into host than count of specified cam_ids, node will not die, and now node publishes empty intrinsics in camerainfo msg when not provided, a bug from pr #33 where default was to not publish info msg (#36)

params/stereo_camera_example.yaml

@@ -0,0 +1,42 @@
+cam_ids:
+- 17197559
+- 17197547
+cam_aliases:
+- cam0
+- cam1
+master_cam: 17197559
+skip: 20
+delay: 1.0
+
+# Assign all the follwing via launch file to prevent confusion and conflict
+
+#save_path: ~/projects/data
+#save_type: .bmp #binary or .tiff or .bmp
+#binning: 1 # going from 2 to 1 requires cameras to be unplugged and replugged
+#color: false
+#frames: 50
+#soft_framerate: 20 # this frame rate reflects to the software frame rate set using ros::rate
+#exp: 997
+#to_ros: true  #When to_ros is not selected, but live is selected, pressing 'space' exports single image to ROS
+
+#Camera info message details
+distortion_model: plumb_bob
+image_height: 1536
+image_width: 2048
+distortion_coeffs:
+- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725]
+- [-0.021141875266089592, -0.3733872931278025, 2.385982550579459, 3.2824571732099725]
+
+
+#specified as [fx  0 cx 0 fy cy 0  0  1]
+intrinsic_coeffs:
+- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0]
+- [1886.9232141485886, 0.0, 604.7214878709341, 0.0, 1886.6668765711668, 493.47726714719823, 0.0, 0.0, 1.0]
+
+rectification_coeffs:
+- [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+- [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+
+projection_coeffs:
+- [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
+- [913.700317, 0.000000, 953.448302, 0.000000, 0.000000, 1063.296631, 777.871993, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

params/test_params.yaml

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

src/capture.cpp

@@ -251,24 +251,30 @@ void acquisition::Capture::load_cameras() {
                 //camera_info_pubs.push_back(nh_.advertise<sensor_msgs::CameraInfo>("camera_array/"+cam_names_[j]+"/camera_info", 1));
 
                 img_msgs.push_back(sensor_msgs::ImagePtr());
+
+                sensor_msgs::CameraInfoPtr ci_msg(new sensor_msgs::CameraInfo());
+                int image_width = 0;
+                int image_height = 0;
+                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
+                ci_msg->binning_x = binning_;
+                ci_msg->binning_y = binning_;
+
                 if (PUBLISH_CAM_INFO_){
-                    sensor_msgs::CameraInfoPtr ci_msg(new sensor_msgs::CameraInfo());
-                    int image_width = 0;
-                    int image_height = 0;
-                    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;
                     ci_msg->D = distortion_coeff_vec_[j];
                     // intrinsic coefficients
-                    for (int count = 0; count<intrinsic_coeff_vec_[j].size();count++)
+                    for (int count = 0; count<intrinsic_coeff_vec_[j].size();count++){
                         ci_msg->K[count] = intrinsic_coeff_vec_[j][count];
+                    }
                     // Rectification matrix
                     if (!rect_coeff_vec_.empty()) 
                         ci_msg->R = {
@@ -287,7 +293,8 @@ void acquisition::Capture::load_cameras() {
                             proj_coeff_vec_[j][8], proj_coeff_vec_[j][9], 
                             proj_coeff_vec_[j][10], proj_coeff_vec_[j][11]};
                     }
-                    else if(numCameras_ == 1){
+                    //else if(numCameras_ == 1){
+                    else if(cam_ids_.size() == 1){  
                         // for case of monocular camera, P[1:3,1:3]=K
                         ci_msg->P = {
                         intrinsic_coeff_vec_[j][0], intrinsic_coeff_vec_[j][1],
@@ -297,19 +304,23 @@ void acquisition::Capture::load_cameras() {
                         intrinsic_coeff_vec_[j][6], intrinsic_coeff_vec_[j][7],
                         intrinsic_coeff_vec_[j][8], 0};
                     }
-                    // binning
-                    ci_msg->binning_x = binning_;
-                    ci_msg->binning_y = binning_;
-
-                    cam_info_msgs.push_back(ci_msg);
                 }
+
+                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.
+    numCameras_ = cams.size();
+    // setting PUBLISH_CAM_INFO_ to true so export to ros method can publish it_.advertiseCamera msg with zero intrisics and distortion coeffs.
+    PUBLISH_CAM_INFO_ = true;
 }
 
 
@@ -558,7 +569,7 @@ void acquisition::Capture::read_parameters() {
     if (PUBLISH_CAM_INFO_)
         ROS_INFO("  Camera coeffs provided, camera info messges will be published.");
     else
-        ROS_INFO("  Camera coeffs not provided correctly, camera info messges will not be published.");
+        ROS_WARN("  Camera coeffs not provided correctly, camera info messges intrinsics and distortion coeffs will be published with zeros.");
 
 //    ROS_ASSERT_MSG(my_list.getType()
 //    int num_ids = cam_id_vec.size();