Added camera_info publishing synced with image publishing.

calib/basler_8mm/ost.txt

@@ -0,0 +1,31 @@
+# oST version 5.0 parameters
+
+
+[image]
+
+width
+1624
+
+height
+1234
+
+[narrow_stereo]
+
+camera matrix
+1918.412263 0.000000 831.707689
+0.000000 1912.356805 636.479080
+0.000000 0.000000 1.000000
+
+distortion
+-0.109684 0.374537 -0.003725 0.002179 0.000000
+
+rectification
+1.000000 0.000000 0.000000
+0.000000 1.000000 0.000000
+0.000000 0.000000 1.000000
+
+projection
+1922.024902 0.000000 833.571480 0.000000
+0.000000 1912.502563 632.790241 0.000000
+0.000000 0.000000 1.000000 0.000000
+

calib/basler_8mm/ost.yaml

@@ -0,0 +1,20 @@
+image_width: 1624
+image_height: 1234
+camera_name: narrow_stereo
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [1918.412263, 0.000000, 831.707689, 0.000000, 1912.356805, 636.479080, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.109684, 0.374537, -0.003725, 0.002179, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [1922.024902, 0.000000, 833.571480, 0.000000, 0.000000, 1912.502563, 632.790241, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

config/default.yaml

@@ -10,7 +10,7 @@ device_user_id: ""
 #  from the ROS-CameraInfoManager:
 #  http://docs.ros.org/api/camera_info_manager/html/classcamera__info__manager_
 #  1_1CameraInfoManager.html#details
-camera_info_url: ""
+camera_info_url: "package://pylon_camera/calib/basler_8mm/ost.yaml"
 
 #  The encoding of the pixels -- channel meaning, ordering, size
 #  taken from the list of strings in include/sensor_msgs/image_encodings.h

include/pylon_camera/pylon_camera_node.h

@@ -332,7 +332,9 @@ class PylonCameraNode
     image_transport::ImageTransport* it_;
     image_transport::CameraPublisher img_raw_pub_;
 
-    ros::Publisher* img_rect_pub_;
+    image_transport::ImageTransport* it_rect_;
+    image_transport::CameraPublisher img_rect_pub_;
+
     image_geometry::PinholeCameraModel* pinhole_model_;
 
     GrabImagesAS grab_imgs_raw_as_;

src/pylon_camera/pylon_camera_node.cpp

@@ -65,7 +65,8 @@ PylonCameraNode::PylonCameraNode()
       pylon_camera_(nullptr),
       it_(new image_transport::ImageTransport(nh_)),
       img_raw_pub_(it_->advertiseCamera("image_raw", 1)),
-      img_rect_pub_(nullptr),
+      it_rect_(new image_transport::ImageTransport(nh_)),
+      img_rect_pub_(it_rect_->advertiseCamera("image_rect", 1)),
       grab_imgs_raw_as_(
               nh_,
               "grab_images_raw",
@@ -355,9 +356,6 @@ bool PylonCameraNode::startGrabbing()
 
 void PylonCameraNode::setupRectification()
 {
-    img_rect_pub_ =
-        new ros::Publisher(nh_.advertise<sensor_msgs::Image>("image_rect", 1));
-
     grab_imgs_rect_as_ =
         new GrabImagesAS(nh_,
                          "grab_images_rect",
@@ -396,23 +394,26 @@ void PylonCameraNode::spin()
         {
             return;
         }
+
+        sensor_msgs::CameraInfoPtr cam_info (new sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo()));
 
-        if ( img_raw_pub_.getNumSubscribers() > 0 )
+
+        if ( img_raw_pub_.getNumSubscribers() + getNumSubscribersRect() > 0 )
         {
             // get actual cam_info-object in every frame, because it might have
             // changed due to a 'set_camera_info'-service call
-            sensor_msgs::CameraInfoPtr cam_info(
-                        new sensor_msgs::CameraInfo(
-                                        camera_info_manager_->getCameraInfo()));
             cam_info->header.stamp = img_raw_msg_.header.stamp;
 
+        }
+        if ( img_raw_pub_.getNumSubscribers() > 0 )
+        {
             // Publish via image_transport
             img_raw_pub_.publish(img_raw_msg_, *cam_info);
         }
 
         if ( getNumSubscribersRect() > 0 )
         {
-            img_rect_pub_->publish(*cv_bridge_img_rect_);
+            img_rect_pub_.publish(*(cv_bridge_img_rect_->toImageMsg()), *cam_info);
         }
     }
 }
@@ -818,12 +819,12 @@ const std::string& PylonCameraNode::cameraFrame() const
 
 uint32_t PylonCameraNode::getNumSubscribersRect() const
 {
-    return camera_info_manager_->isCalibrated() ? img_rect_pub_->getNumSubscribers() : 0;
+    return camera_info_manager_->isCalibrated() ? img_rect_pub_.getNumSubscribers() : 0;
 }
 
 uint32_t PylonCameraNode::getNumSubscribers() const
 {
-    return img_raw_pub_.getNumSubscribers() + img_rect_pub_->getNumSubscribers();
+    return img_raw_pub_.getNumSubscribers() + img_rect_pub_.getNumSubscribers();
 }
 
 void PylonCameraNode::setupInitialCameraInfo(sensor_msgs::CameraInfo& cam_info_msg)
@@ -1520,6 +1521,8 @@ PylonCameraNode::~PylonCameraNode()
     pylon_camera_ = NULL;
     delete it_;
     it_ = NULL;
+    delete it_rect_;
+    it_rect_ = NULL;
 }
 
 }  // namespace pylon_camera