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